From dfd22d0f13496772f62633f1057d51b43d1670cb Mon Sep 17 00:00:00 2001 From: Christian Henkel Date: Thu, 4 Jul 2024 17:51:07 +0200 Subject: [PATCH 01/51] markdown linter Signed-off-by: Christian Henkel --- .github/worklows/md-lint.yml | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) create mode 100644 .github/worklows/md-lint.yml diff --git a/.github/worklows/md-lint.yml b/.github/worklows/md-lint.yml new file mode 100644 index 0000000..f71fea9 --- /dev/null +++ b/.github/worklows/md-lint.yml @@ -0,0 +1,17 @@ +name: Lint +on: + push: + branches: [main] + pull_request: + branches: [main] +jobs: + md-lint: + name: Lint README.md + runs-on: ubuntu-latest + steps: + - name: Check out code + uses: actions/checkout@v2 + - name: Lint markdown files + uses: avto-dev/markdown-lint@v1.5.0 + with: + files: README.md From d6921071f45c04a30ed3b3264240bf2c78ba7c26 Mon Sep 17 00:00:00 2001 From: Christian Henkel Date: Thu, 4 Jul 2024 17:52:46 +0200 Subject: [PATCH 02/51] workflows Signed-off-by: Christian Henkel --- .github/{worklows => workflows}/md-lint.yml | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename .github/{worklows => workflows}/md-lint.yml (100%) diff --git a/.github/worklows/md-lint.yml b/.github/workflows/md-lint.yml similarity index 100% rename from .github/worklows/md-lint.yml rename to .github/workflows/md-lint.yml From 9f96a9978c281ad114dc032d8868b995c86c0a1c Mon Sep 17 00:00:00 2001 From: Christian Henkel Date: Thu, 4 Jul 2024 17:54:29 +0200 Subject: [PATCH 03/51] args Signed-off-by: Christian Henkel --- .github/workflows/md-lint.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/md-lint.yml b/.github/workflows/md-lint.yml index f71fea9..b36d869 100644 --- a/.github/workflows/md-lint.yml +++ b/.github/workflows/md-lint.yml @@ -14,4 +14,4 @@ jobs: - name: Lint markdown files uses: avto-dev/markdown-lint@v1.5.0 with: - files: README.md + args: README.md From 162b87ea157bd188d926167758f14ed232f4f300 Mon Sep 17 00:00:00 2001 From: Christian Henkel Date: Thu, 4 Jul 2024 17:57:16 +0200 Subject: [PATCH 04/51] readme fixes Signed-off-by: Christian Henkel --- README.md | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/README.md b/README.md index 564eafd..d57573e 100644 --- a/README.md +++ b/README.md @@ -1,35 +1,34 @@ # Hands-On with ROS 2 Deliberation Technologies -WORK IN PROGRESS! +> [!WARNING] +> WORK IN PROGRESS! -This repository contains materials for the ROSCon 2024 workshop on ROS 2 Deliberation Technologies. - ---- +This repository contains materials for the ROSCon 2024 workshop on ROS 2 +Deliberation Technologies. ## Setup First, clone this repository. -``` +```bash git clone -b https://github.com/ros-wg-delib/roscon24-workshop.git ``` Then, build the Docker image. -``` +```bash docker compose build ``` Start a container. -``` +```bash docker compose run base ``` Once you're in the container, check that you can run a demo. -``` +```bash ros2 launch pyrobosim_ros demo.launch.py ``` ---- From 1f122cc1672a6cc90ebb7db7daa9991ffbde59c8 Mon Sep 17 00:00:00 2001 From: Christian Henkel Date: Thu, 4 Jul 2024 18:06:30 +0200 Subject: [PATCH 05/51] draft of problems Signed-off-by: Christian Henkel --- README.md | 111 ++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 111 insertions(+) diff --git a/README.md b/README.md index d57573e..7c8b93e 100644 --- a/README.md +++ b/README.md @@ -32,3 +32,114 @@ Once you're in the container, check that you can run a demo. ros2 launch pyrobosim_ros demo.launch.py ``` +## Problem Descriptions + +### Task 1 + +__Goal__: +Banana on table 2. +__Initial State__: +Banana on table 1. +__Available Actions__: + +- Pick object +- Place object +- Move robot + +__Available Conditions__: + +- Robot location + table 1, table 2 +- Object location + table 1, table 2 +- Object in hand + true, false + +### Task 2 + +__Goal__: +Banana on table 2. +__Initial State__: +Banana on table 1. +__Available Actions__: + +- Pick object +- Place object +- Move robot + (will fail if door is closed) +- Open door +- Close door + +__Available Conditions__: + +- Robot location + table 1, table 2 +- Object location + table 1, table 2 +- Object in hand + true, false +- Door state + open, closed + +### Task 3 + +__Goal__: +Banana on table 2. +__Initial State__: +Banana on table 1. +__Available Actions__: + +- Pick object + (may fail with probability 0.5) +- Place object +- Move robot + Locations: table 1, table 2, door + (will fail if door is closed) +- Open door + (may fail with probability 0.5) +- Close door + +__Available Conditions__: + +- Robot location + table 1, table 2 +- Object location + table 1, table 2 +- Object in hand + true, false +- Door state + open, closed + +### Task 4 + +__Goal__: +Banana on table 2. +Never run out of battery. +__Initial State__: +Banana on table 1. +__Available Actions__: + +- Pick object + (may fail with probability 0.5) +- Place object +- Move robot + Locations: table 1, table 2, door, charging station + (will fail if door is closed) +- Open door + (may fail with probability 0.5) +- Close door +- Charge battery + (can only be done at charging station) + +__Available Conditions__: + +- Robot location + table 1, table 2 +- Object location + table 1, table 2 +- Object in hand + true, false +- Door state + open, closed +- Battery level + (low, normal, full) \ No newline at end of file From 50cef9d1cca9ce5423f69741819278550852e207 Mon Sep 17 00:00:00 2001 From: Christian Henkel Date: Thu, 4 Jul 2024 18:07:39 +0200 Subject: [PATCH 06/51] linter Signed-off-by: Christian Henkel --- README.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index 7c8b93e..49b2787 100644 --- a/README.md +++ b/README.md @@ -36,9 +36,9 @@ ros2 launch pyrobosim_ros demo.launch.py ### Task 1 -__Goal__: +__Goal__: Banana on table 2. -__Initial State__: +__Initial State__: Banana on table 1. __Available Actions__: @@ -142,4 +142,4 @@ __Available Conditions__: - Door state open, closed - Battery level - (low, normal, full) \ No newline at end of file + (low, normal, full) From 66d56aa9fbf12409416f1fc44b13ff0d02cbc45f Mon Sep 17 00:00:00 2001 From: Christian Henkel Date: Thu, 4 Jul 2024 18:08:48 +0200 Subject: [PATCH 07/51] newlines Signed-off-by: Christian Henkel --- README.md | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/README.md b/README.md index 49b2787..c7b7264 100644 --- a/README.md +++ b/README.md @@ -59,8 +59,10 @@ __Available Conditions__: __Goal__: Banana on table 2. + __Initial State__: Banana on table 1. + __Available Actions__: - Pick object @@ -85,8 +87,10 @@ __Available Conditions__: __Goal__: Banana on table 2. + __Initial State__: Banana on table 1. + __Available Actions__: - Pick object @@ -115,8 +119,10 @@ __Available Conditions__: __Goal__: Banana on table 2. Never run out of battery. + __Initial State__: Banana on table 1. + __Available Actions__: - Pick object From 54973cea0ba8008164913bd84b63ba01e2414538 Mon Sep 17 00:00:00 2001 From: Christian Henkel Date: Tue, 9 Jul 2024 11:50:46 +0200 Subject: [PATCH 08/51] changes from meeting Signed-off-by: Christian Henkel --- README.md | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/README.md b/README.md index c7b7264..2e70850 100644 --- a/README.md +++ b/README.md @@ -38,8 +38,10 @@ ros2 launch pyrobosim_ros demo.launch.py __Goal__: Banana on table 2. + __Initial State__: Banana on table 1. + __Available Actions__: - Pick object @@ -99,6 +101,7 @@ __Available Actions__: - Move robot Locations: table 1, table 2, door (will fail if door is closed) + (can also fail while moving with probability 0.1) - Open door (may fail with probability 0.5) - Close door @@ -131,8 +134,12 @@ __Available Actions__: - Move robot Locations: table 1, table 2, door, charging station (will fail if door is closed) + (can also fail while moving with probability 0.1) - Open door (may fail with probability 0.5) + (returns reason for failure, slipped, locked) + - locked -> find different way + - slipped handle -> try again - Close door - Charge battery (can only be done at charging station) From 100cdd2501b0e1cf9a1d6b40dac926508a4c7a6e Mon Sep 17 00:00:00 2001 From: Christian Henkel Date: Wed, 10 Jul 2024 21:51:41 +0200 Subject: [PATCH 09/51] right folder Signed-off-by: Christian Henkel --- .github/{ => workflows}/ros_test.yml | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename .github/{ => workflows}/ros_test.yml (100%) diff --git a/.github/ros_test.yml b/.github/workflows/ros_test.yml similarity index 100% rename from .github/ros_test.yml rename to .github/workflows/ros_test.yml From 71ea73960d5a2236edbceb6741c10dd113c4f594 Mon Sep 17 00:00:00 2001 From: Christian Henkel Date: Sun, 14 Jul 2024 11:35:17 +0200 Subject: [PATCH 10/51] simplifying model data Signed-off-by: Christian Henkel --- delib_ws_p1/data/location_data.yaml | 18 ++---------------- delib_ws_p1/data/object_data.yaml | 24 ------------------------ delib_ws_p1/delib_ws_p1/run.py | 4 ++-- 3 files changed, 4 insertions(+), 42 deletions(-) diff --git a/delib_ws_p1/data/location_data.yaml b/delib_ws_p1/data/location_data.yaml index c148aee..573c106 100644 --- a/delib_ws_p1/data/location_data.yaml +++ b/delib_ws_p1/data/location_data.yaml @@ -2,7 +2,7 @@ # Example location metadata # ############################# -table_source: +table: footprint: type: box dims: [0.6, 0.6] @@ -14,19 +14,5 @@ table_source: footprint: type: parent padding: 0.1 - color: [0.2, 0, 0] - -table_sink: - footprint: - type: box - dims: [0.6, 0.6] - height: 0.5 - nav_poses: - - [0, -0.4, 1.57] - locations: - - name: "tabletop" - footprint: - type: parent - padding: 0.1 - color: [0, 0.2, 0] + color: [0.2, 0.2, 0.2] diff --git a/delib_ws_p1/data/object_data.yaml b/delib_ws_p1/data/object_data.yaml index 2261203..2396f0f 100644 --- a/delib_ws_p1/data/object_data.yaml +++ b/delib_ws_p1/data/object_data.yaml @@ -2,32 +2,8 @@ # Example object metadata # ########################### -apple: - footprint: - type: circle - radius: 0.06 - color: [1, 0, 0] - banana: footprint: type: box dims: [0.05, 0.2] color: [0.7, 0.7, 0] - -water: - footprint: - type: polygon - coords: - - [0.035, -0.075] - - [0.035, 0.075] - - [0.0, 0.1] - - [-0.035, 0.075] - - [-0.035, -0.075] - color: [0.0, 0.1, 0.7] - -coke: - footprint: - type: mesh - model_path: $DATA/sample_models/coke_can - mesh_path: meshes/coke_can.dae - color: [0.8, 0, 0] diff --git a/delib_ws_p1/delib_ws_p1/run.py b/delib_ws_p1/delib_ws_p1/run.py index c0085d2..83e2fc3 100644 --- a/delib_ws_p1/delib_ws_p1/run.py +++ b/delib_ws_p1/delib_ws_p1/run.py @@ -42,14 +42,14 @@ def create_world(): # Add locations table_source = world.add_location( - category="table_source", + category="table", parent="banana_farm", name="table_source", pose=Pose(x=-1.5, y=0.5) ) table = world.add_location( - category="table_sink", + category="table", parent="dining_room", name="table_sink", pose=Pose(x=1.5, y=0.5) From 754408bd206a5be37d59b31fa5c693007cd1b937 Mon Sep 17 00:00:00 2001 From: Christian Henkel Date: Sun, 14 Jul 2024 11:39:34 +0200 Subject: [PATCH 11/51] remove unused import Signed-off-by: Christian Henkel --- delib_ws_p1/delib_ws_p1/run.py | 1 - 1 file changed, 1 deletion(-) diff --git a/delib_ws_p1/delib_ws_p1/run.py b/delib_ws_p1/delib_ws_p1/run.py index 83e2fc3..e8c504d 100644 --- a/delib_ws_p1/delib_ws_p1/run.py +++ b/delib_ws_p1/delib_ws_p1/run.py @@ -12,7 +12,6 @@ from pyrobosim.core import Robot, World, WorldYamlLoader from pyrobosim.gui import start_gui from pyrobosim.navigation import ConstantVelocityExecutor, PathPlanner -from pyrobosim.utils.general import get_data_folder from pyrobosim.utils.pose import Pose from pyrobosim_ros.ros_interface import WorldROSWrapper from ament_index_python.packages import get_package_share_directory From f82ed8efcd5e96c29d5123c26e702bba6c05d2ac Mon Sep 17 00:00:00 2001 From: Christian Henkel Date: Sun, 14 Jul 2024 11:41:27 +0200 Subject: [PATCH 12/51] docstring Signed-off-by: Christian Henkel --- delib_ws_p1/delib_ws_p1/run.py | 23 +++++++++++++++++++++-- 1 file changed, 21 insertions(+), 2 deletions(-) diff --git a/delib_ws_p1/delib_ws_p1/run.py b/delib_ws_p1/delib_ws_p1/run.py index e8c504d..68d11db 100644 --- a/delib_ws_p1/delib_ws_p1/run.py +++ b/delib_ws_p1/delib_ws_p1/run.py @@ -1,8 +1,27 @@ #!/usr/bin/env python3 """ -Example showing how to build a world and use it with pyrobosim, -additionally starting up a ROS interface. +__Goal__: +Banana on table_sink. + +__Initial State__: +Banana on table_source. + +__Available Actions__: + +- Pick object +- Place object +- Move robot + +__Available Conditions__: + +- Robot location + dining_room, banana_farm +- Object location + table_sink, table_source +- Object in hand + true, false + """ import os import rclpy From e88c7741b4b23b7f62b87e16f5eece2cbd0b5f0b Mon Sep 17 00:00:00 2001 From: Christian Henkel Date: Sun, 14 Jul 2024 11:42:05 +0200 Subject: [PATCH 13/51] license file on repo level Signed-off-by: Christian Henkel --- delib_ws_p1/LICENSE => LICENSE | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename delib_ws_p1/LICENSE => LICENSE (100%) diff --git a/delib_ws_p1/LICENSE b/LICENSE similarity index 100% rename from delib_ws_p1/LICENSE rename to LICENSE From efc240d7c2916a650f8202a34de6610410213090 Mon Sep 17 00:00:00 2001 From: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Date: Wed, 14 Aug 2024 21:21:10 -0400 Subject: [PATCH 14/51] Update docker setup to use workshop files correctly (#5) --- docker-compose.yaml | 6 +++--- docker/Dockerfile | 9 ++------- src/.gitkeep | 1 - 3 files changed, 5 insertions(+), 11 deletions(-) delete mode 100644 src/.gitkeep diff --git a/docker-compose.yaml b/docker-compose.yaml index 6341b00..5c5a33b 100644 --- a/docker-compose.yaml +++ b/docker-compose.yaml @@ -27,14 +27,14 @@ services: # Networking and IPC for ROS 2 network_mode: host ipc: host - # Allows graphical programs in the container. + # Allows graphical programs in the container environment: - DISPLAY=${DISPLAY} - QT_X11_NO_MITSHM=1 - NVIDIA_DRIVER_CAPABILITIES=all volumes: # Mount the workshop source code - - ./src:/delib_ws/src/workshop_files:rw - # Allows graphical programs in the container. + - ./:/delib_ws/src/workshop_files:rw + # Allows graphical programs in the container - /tmp/.X11-unix:/tmp/.X11-unix:rw - ${XAUTHORITY:-$HOME/.Xauthority}:/root/.Xauthority diff --git a/docker/Dockerfile b/docker/Dockerfile index 12c8a46..64f153a 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -14,8 +14,7 @@ RUN apt-get install -y \ # Create a ROS 2 workspace RUN mkdir -p /delib_ws/src/ -# Install pyrobosim -# TODO: Should we clone it this way, or include it as a submodule in the repo? +# Install pyrobosim and its dependencies WORKDIR /delib_ws/src RUN git clone -b main https://github.com/sea-bass/pyrobosim.git ENV PIP_BREAK_SYSTEM_PACKAGES=1 @@ -23,11 +22,6 @@ RUN cd pyrobosim && \ pip3 install -e ./pyrobosim && \ pip3 install -r test/python_test_requirements.txt -# Build the ROS workspace -WORKDIR /delib_ws -RUN . /opt/ros/jazzy/setup.bash && \ - colcon build - # Remove MatPlotLib display warnings RUN mkdir /tmp/runtime-root ENV XDG_RUNTIME_DIR "/tmp/runtime-root" @@ -35,6 +29,7 @@ RUN chmod -R 0700 /tmp/runtime-root ENV NO_AT_BRIDGE 1 # Set up the entrypoint +WORKDIR /delib_ws CMD /bin/bash COPY docker/entrypoint.sh /entrypoint.sh ENTRYPOINT ["/entrypoint.sh"] diff --git a/src/.gitkeep b/src/.gitkeep deleted file mode 100644 index 2a6fd2c..0000000 --- a/src/.gitkeep +++ /dev/null @@ -1 +0,0 @@ -This is where the real workshop contents will go. From c06bf4bc0bd5d1869940126f4ab25063a3068ede Mon Sep 17 00:00:00 2001 From: Christian Henkel <6976069+ct2034@users.noreply.github.com> Date: Sun, 18 Aug 2024 15:15:04 +0200 Subject: [PATCH 15/51] All problems and some python solutions (#6) Signed-off-by: Christian Henkel Co-authored-by: Sebastian Castro --- .github/format.yml | 15 -- .github/workflows/{md-lint.yml => lint.yml} | 9 + .github/workflows/ros_test.yml | 57 ++--- .pre-commit-config.yaml | 4 +- README.md | 20 +- delib_ws_p1/data/location_data.yaml | 1 - delib_ws_p1/delib_ws_p1/is_at_goal.py | 14 ++ delib_ws_p1/delib_ws_p1/run.py | 10 +- delib_ws_p1/package.xml | 7 +- delib_ws_p1/setup.py | 30 +-- delib_ws_p1/test/test_copyright.py | 8 +- delib_ws_p1/test/test_flake8.py | 6 +- delib_ws_p1/test/test_pep257.py | 4 +- delib_ws_p1_py/LICENSE | 202 ++++++++++++++++++ delib_ws_p1_py/delib_ws_p1_py/__init__.py | 0 delib_ws_p1_py/delib_ws_p1_py/run.py | 31 +++ delib_ws_p1_py/launch/run.launch.py | 17 ++ delib_ws_p1_py/package.xml | 21 ++ delib_ws_p1_py/resource/delib_ws_p1_py | 0 delib_ws_p1_py/setup.cfg | 4 + delib_ws_p1_py/setup.py | 27 +++ delib_ws_p1_py/test/test_copyright.py | 27 +++ delib_ws_p1_py/test/test_flake8.py | 25 +++ delib_ws_p1_py/test/test_pep257.py | 23 ++ delib_ws_p2/data/location_data.yaml | 17 ++ delib_ws_p2/data/object_data.yaml | 9 + delib_ws_p2/data/world.yaml | 57 +++++ delib_ws_p2/delib_ws_p2/__init__.py | 0 delib_ws_p2/delib_ws_p2/is_at_goal.py | 14 ++ delib_ws_p2/delib_ws_p2/run.py | 92 ++++++++ delib_ws_p2/package.xml | 21 ++ delib_ws_p2/resource/delib_ws_p2 | 0 delib_ws_p2/setup.cfg | 4 + delib_ws_p2/setup.py | 29 +++ delib_ws_p2/test/test_copyright.py | 27 +++ delib_ws_p2/test/test_flake8.py | 25 +++ delib_ws_p2/test/test_pep257.py | 23 ++ delib_ws_p2_py/LICENSE | 202 ++++++++++++++++++ delib_ws_p2_py/delib_ws_p2_py/__init__.py | 0 delib_ws_p2_py/delib_ws_p2_py/run.py | 33 +++ delib_ws_p2_py/launch/run.launch.py | 15 ++ delib_ws_p2_py/package.xml | 21 ++ delib_ws_p2_py/resource/delib_ws_p2_py | 0 delib_ws_p2_py/setup.cfg | 4 + delib_ws_p2_py/setup.py | 27 +++ delib_ws_p2_py/test/test_copyright.py | 27 +++ delib_ws_p2_py/test/test_flake8.py | 25 +++ delib_ws_p2_py/test/test_pep257.py | 23 ++ delib_ws_p3/data/location_data.yaml | 17 ++ delib_ws_p3/data/object_data.yaml | 9 + delib_ws_p3/data/world.yaml | 67 ++++++ delib_ws_p3/delib_ws_p3/__init__.py | 0 delib_ws_p3/delib_ws_p3/is_at_goal.py | 14 ++ delib_ws_p3/delib_ws_p3/run.py | 92 ++++++++ delib_ws_p3/package.xml | 21 ++ delib_ws_p3/resource/delib_ws_p3 | 0 delib_ws_p3/setup.cfg | 4 + delib_ws_p3/setup.py | 29 +++ delib_ws_p3/test/test_copyright.py | 27 +++ delib_ws_p3/test/test_flake8.py | 25 +++ delib_ws_p3/test/test_pep257.py | 23 ++ delib_ws_p4/data/location_data.yaml | 17 ++ delib_ws_p4/data/object_data.yaml | 9 + delib_ws_p4/data/world.yaml | 72 +++++++ delib_ws_p4/delib_ws_p4/__init__.py | 0 delib_ws_p4/delib_ws_p4/is_at_goal.py | 14 ++ delib_ws_p4/delib_ws_p4/run.py | 92 ++++++++ delib_ws_p4/package.xml | 21 ++ delib_ws_p4/resource/delib_ws_p4 | 0 delib_ws_p4/setup.cfg | 4 + delib_ws_p4/setup.py | 29 +++ delib_ws_p4/test/test_copyright.py | 27 +++ delib_ws_p4/test/test_flake8.py | 25 +++ delib_ws_p4/test/test_pep257.py | 23 ++ problem_interface/LICENSE | 202 ++++++++++++++++++ problem_interface/package.xml | 21 ++ .../problem_interface/__init__.py | 0 .../problem_interface/perform_action.py | 89 ++++++++ .../problem_interface/world_state.py | 95 ++++++++ problem_interface/resource/problem_interface | 0 problem_interface/setup.cfg | 4 + problem_interface/setup.py | 23 ++ problem_interface/test/test_copyright.py | 27 +++ problem_interface/test/test_flake8.py | 25 +++ problem_interface/test/test_pep257.py | 23 ++ 85 files changed, 2328 insertions(+), 99 deletions(-) delete mode 100644 .github/format.yml rename .github/workflows/{md-lint.yml => lint.yml} (64%) create mode 100644 delib_ws_p1/delib_ws_p1/is_at_goal.py create mode 100644 delib_ws_p1_py/LICENSE create mode 100644 delib_ws_p1_py/delib_ws_p1_py/__init__.py create mode 100644 delib_ws_p1_py/delib_ws_p1_py/run.py create mode 100644 delib_ws_p1_py/launch/run.launch.py create mode 100644 delib_ws_p1_py/package.xml create mode 100644 delib_ws_p1_py/resource/delib_ws_p1_py create mode 100644 delib_ws_p1_py/setup.cfg create mode 100644 delib_ws_p1_py/setup.py create mode 100644 delib_ws_p1_py/test/test_copyright.py create mode 100644 delib_ws_p1_py/test/test_flake8.py create mode 100644 delib_ws_p1_py/test/test_pep257.py create mode 100644 delib_ws_p2/data/location_data.yaml create mode 100644 delib_ws_p2/data/object_data.yaml create mode 100644 delib_ws_p2/data/world.yaml create mode 100644 delib_ws_p2/delib_ws_p2/__init__.py create mode 100644 delib_ws_p2/delib_ws_p2/is_at_goal.py create mode 100644 delib_ws_p2/delib_ws_p2/run.py create mode 100644 delib_ws_p2/package.xml create mode 100644 delib_ws_p2/resource/delib_ws_p2 create mode 100644 delib_ws_p2/setup.cfg create mode 100644 delib_ws_p2/setup.py create mode 100644 delib_ws_p2/test/test_copyright.py create mode 100644 delib_ws_p2/test/test_flake8.py create mode 100644 delib_ws_p2/test/test_pep257.py create mode 100644 delib_ws_p2_py/LICENSE create mode 100644 delib_ws_p2_py/delib_ws_p2_py/__init__.py create mode 100644 delib_ws_p2_py/delib_ws_p2_py/run.py create mode 100644 delib_ws_p2_py/launch/run.launch.py create mode 100644 delib_ws_p2_py/package.xml create mode 100644 delib_ws_p2_py/resource/delib_ws_p2_py create mode 100644 delib_ws_p2_py/setup.cfg create mode 100644 delib_ws_p2_py/setup.py create mode 100644 delib_ws_p2_py/test/test_copyright.py create mode 100644 delib_ws_p2_py/test/test_flake8.py create mode 100644 delib_ws_p2_py/test/test_pep257.py create mode 100644 delib_ws_p3/data/location_data.yaml create mode 100644 delib_ws_p3/data/object_data.yaml create mode 100644 delib_ws_p3/data/world.yaml create mode 100644 delib_ws_p3/delib_ws_p3/__init__.py create mode 100644 delib_ws_p3/delib_ws_p3/is_at_goal.py create mode 100644 delib_ws_p3/delib_ws_p3/run.py create mode 100644 delib_ws_p3/package.xml create mode 100644 delib_ws_p3/resource/delib_ws_p3 create mode 100644 delib_ws_p3/setup.cfg create mode 100644 delib_ws_p3/setup.py create mode 100644 delib_ws_p3/test/test_copyright.py create mode 100644 delib_ws_p3/test/test_flake8.py create mode 100644 delib_ws_p3/test/test_pep257.py create mode 100644 delib_ws_p4/data/location_data.yaml create mode 100644 delib_ws_p4/data/object_data.yaml create mode 100644 delib_ws_p4/data/world.yaml create mode 100644 delib_ws_p4/delib_ws_p4/__init__.py create mode 100644 delib_ws_p4/delib_ws_p4/is_at_goal.py create mode 100644 delib_ws_p4/delib_ws_p4/run.py create mode 100644 delib_ws_p4/package.xml create mode 100644 delib_ws_p4/resource/delib_ws_p4 create mode 100644 delib_ws_p4/setup.cfg create mode 100644 delib_ws_p4/setup.py create mode 100644 delib_ws_p4/test/test_copyright.py create mode 100644 delib_ws_p4/test/test_flake8.py create mode 100644 delib_ws_p4/test/test_pep257.py create mode 100644 problem_interface/LICENSE create mode 100644 problem_interface/package.xml create mode 100644 problem_interface/problem_interface/__init__.py create mode 100644 problem_interface/problem_interface/perform_action.py create mode 100644 problem_interface/problem_interface/world_state.py create mode 100644 problem_interface/resource/problem_interface create mode 100644 problem_interface/setup.cfg create mode 100644 problem_interface/setup.py create mode 100644 problem_interface/test/test_copyright.py create mode 100644 problem_interface/test/test_flake8.py create mode 100644 problem_interface/test/test_pep257.py diff --git a/.github/format.yml b/.github/format.yml deleted file mode 100644 index 017de18..0000000 --- a/.github/format.yml +++ /dev/null @@ -1,15 +0,0 @@ -name: format - -on: - # Run action on certain pull request events - pull_request: - types: [opened, synchronize, reopened, ready_for_review] - -jobs: - pre-commit: - name: pre-commit - runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v4 - - uses: actions/setup-python@v5 - - uses: pre-commit/action@v3.0.1 diff --git a/.github/workflows/md-lint.yml b/.github/workflows/lint.yml similarity index 64% rename from .github/workflows/md-lint.yml rename to .github/workflows/lint.yml index b36d869..5506b14 100644 --- a/.github/workflows/md-lint.yml +++ b/.github/workflows/lint.yml @@ -4,6 +4,7 @@ on: branches: [main] pull_request: branches: [main] + jobs: md-lint: name: Lint README.md @@ -15,3 +16,11 @@ jobs: uses: avto-dev/markdown-lint@v1.5.0 with: args: README.md + + pre-commit: + name: Run pre-commit + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + - uses: actions/setup-python@v5 + - uses: pre-commit/action@v3.0.1 diff --git a/.github/workflows/ros_test.yml b/.github/workflows/ros_test.yml index 2fdbf8b..f8f6af8 100644 --- a/.github/workflows/ros_test.yml +++ b/.github/workflows/ros_test.yml @@ -13,50 +13,17 @@ jobs: name: ${{ matrix.package }} on ${{ matrix.distro }} strategy: fail-fast: false - matrix: - package: - [ - delib_ws_p1, - pyrobosim_ros, - ] - include: - - distro: jazzy - os: ubuntu-24.04 - runs-on: ${{ matrix.distro }} + runs-on: ubuntu-latest steps: - - uses: ros-tooling/setup-ros@master - # - run: | - # sudo pip install pydocstyle==6.1.1 # downgrade to fix https://github.com/ament/ament_lint/pull/428 - # sudo pip install pip --upgrade - # sudo pip install pyopenssl --upgrade # fix for AttributeError: module 'lib' has no attribute 'X509_V_FLAG_CB_ISSUER_CHECK' - - uses: ros-tooling/action-ros-ci@0.3.5 + - uses: actions/checkout@v2 + - name: Build and start docker + uses: hoverkraft-tech/compose-action@v2.0.1 with: - target-ros2-distro: ${{ matrix.distro }} - package-name: ${{ matrix.package }} - # vcs-repo-file-url: | - # https://raw.githubusercontent.com/ros2/ros2/master/ros2.repos - # - run: | - # rosdep update - # - run: | - # rm -rf * # clean up the workspace - # - uses: actions/checkout@v2 - # with: - # path: src/bt_playground - # - run: | - # git clone https://github.com/boschresearch/bt_tools src/bt_tools - # - run: | - # git clone https://github.com/ros-planning/navigation2 src/navigation2 - # # remove everything except nav2_msgs - # - run: | - # find src/navigation2 -mindepth 1 -maxdepth 1 -type d -not -name 'nav2_msgs' -exec rm -rf {} + - # # verify that ... - # - run: | - # colcon list - # - run: | - # rosdep install --from-paths src --ignore-src --rosdistro ${{ matrix.distro }} -y - # - run: | - # colcon build --packages-up-to ${{ matrix.package }} - # - run: | - # colcon test --packages-select ${{ matrix.package }} - # - run: | - # colcon test-result --all + compose-file: ./docker-compose.yaml + services: base + # TODO: Add the following steps + # - name: Run tests + # run: | + # docker compose exec base colcon build + # docker compose exec base colcon test + # docker compose exec base colcon test-result diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 6910907..a0cf8ff 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -30,7 +30,7 @@ repos: # Autoformats Python code. - repo: https://github.com/psf/black.git - rev: 24.4.2 + rev: 24.8.0 hooks: - id: black @@ -49,7 +49,7 @@ repos: [ "--no-warnings", "--config-data", - "{extends: default, rules: {line-length: disable, braces: {max-spaces-inside: 1}}}", + "{extends: default, rules: {line-length: disable, braces: {max-spaces-inside: 1}, indentation: disable}}", ] types: [text] files: \.(yml|yaml)$ diff --git a/README.md b/README.md index 2e70850..160e448 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # Hands-On with ROS 2 Deliberation Technologies -> [!WARNING] +> [!WARNING] > WORK IN PROGRESS! This repository contains materials for the ROSCon 2024 workshop on ROS 2 @@ -29,12 +29,14 @@ docker compose run base Once you're in the container, check that you can run a demo. ```bash -ros2 launch pyrobosim_ros demo.launch.py +ros2 run delib_ws_p1 run ``` ## Problem Descriptions -### Task 1 +### Problem 1 + +[package delib_ws_p1](delib_ws_p1) __Goal__: Banana on table 2. @@ -57,7 +59,9 @@ __Available Conditions__: - Object in hand true, false -### Task 2 +### Problem 2 + +[package delib_ws_p2](delib_ws_p2) __Goal__: Banana on table 2. @@ -85,7 +89,9 @@ __Available Conditions__: - Door state open, closed -### Task 3 +### Problem 3 + +[package delib_ws_p3](delib_ws_p3) __Goal__: Banana on table 2. @@ -117,7 +123,9 @@ __Available Conditions__: - Door state open, closed -### Task 4 +### Problem 4 + +[package delib_ws_p4](delib_ws_p4) __Goal__: Banana on table 2. diff --git a/delib_ws_p1/data/location_data.yaml b/delib_ws_p1/data/location_data.yaml index 573c106..1282330 100644 --- a/delib_ws_p1/data/location_data.yaml +++ b/delib_ws_p1/data/location_data.yaml @@ -15,4 +15,3 @@ table: type: parent padding: 0.1 color: [0.2, 0.2, 0.2] - diff --git a/delib_ws_p1/delib_ws_p1/is_at_goal.py b/delib_ws_p1/delib_ws_p1/is_at_goal.py new file mode 100644 index 0000000..2f180fb --- /dev/null +++ b/delib_ws_p1/delib_ws_p1/is_at_goal.py @@ -0,0 +1,14 @@ +from problem_interface.world_state import WorldState + +import sys + + +def get_goal_state(): + return [("objects.banana0.parent", "table_sink_tabletop")] + + +def main(): + ws = WorldState() + is_at_goal = ws.get_state(get_goal_state()) + print(is_at_goal) + sys.exit(0) diff --git a/delib_ws_p1/delib_ws_p1/run.py b/delib_ws_p1/delib_ws_p1/run.py index 68d11db..e448c23 100644 --- a/delib_ws_p1/delib_ws_p1/run.py +++ b/delib_ws_p1/delib_ws_p1/run.py @@ -63,19 +63,21 @@ def create_world(): category="table", parent="banana_farm", name="table_source", - pose=Pose(x=-1.5, y=0.5) + pose=Pose(x=-1.5, y=0.5), ) table = world.add_location( category="table", parent="dining_room", name="table_sink", - pose=Pose(x=1.5, y=0.5) + pose=Pose(x=1.5, y=0.5), ) # Add objects world.add_object( - category="banana", parent=table_source, pose=Pose(x=-1.5, y=0.5, yaw=np.pi / 4.0) + category="banana", + parent=table_source, + pose=Pose(x=-1.5, y=0.5, yaw=np.pi / 4.0), ) # Add a robot @@ -137,5 +139,5 @@ def main(): start_gui(node.world) -if __name__ == '__main__': +if __name__ == "__main__": main() diff --git a/delib_ws_p1/package.xml b/delib_ws_p1/package.xml index 5320ca6..a14c943 100644 --- a/delib_ws_p1/package.xml +++ b/delib_ws_p1/package.xml @@ -2,12 +2,13 @@ delib_ws_p1 - 0.0.0 - TODO: Package description - hec2le + 1.0.0 + Problem 1 of the ROSCon24 Deliberation Workshop + Christian Henkel Apache-2.0 pyrobosim_ros + problem_interface ament_copyright ament_flake8 diff --git a/delib_ws_p1/setup.py b/delib_ws_p1/setup.py index 63e8b37..a87f302 100644 --- a/delib_ws_p1/setup.py +++ b/delib_ws_p1/setup.py @@ -2,28 +2,28 @@ from setuptools import find_packages, setup -package_name = 'delib_ws_p1' +package_name = "delib_ws_p1" setup( name=package_name, - version='0.0.0', - packages=find_packages(exclude=['test']), + version="0.0.0", + packages=find_packages(exclude=["test"]), data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), - ('share/' + package_name + "/data", glob('data/*.*')), + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + ("share/" + package_name + "/data", glob("data/*.*")), ], - install_requires=['setuptools'], + install_requires=["setuptools"], zip_safe=True, - maintainer='Christian Henkel', - maintainer_email='christian.henkel2@de.bosch.com', - description='TODO: Package description', - license='Apache-2.0', - tests_require=['pytest'], + maintainer="Christian Henkel", + maintainer_email="christian.henkel2@de.bosch.com", + description="TODO: Package description", + license="Apache-2.0", + tests_require=["pytest"], entry_points={ - 'console_scripts': [ - 'run = delib_ws_p1.run:main' + "console_scripts": [ + "run = delib_ws_p1.run:main", + "is_at_goal = delib_ws_p1.is_at_goal:main", ], }, ) diff --git a/delib_ws_p1/test/test_copyright.py b/delib_ws_p1/test/test_copyright.py index 97a3919..ceffe89 100644 --- a/delib_ws_p1/test/test_copyright.py +++ b/delib_ws_p1/test/test_copyright.py @@ -17,9 +17,11 @@ # Remove the `skip` decorator once the source file(s) have a copyright header -@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.skip( + reason="No copyright header has been placed in the generated source file." +) @pytest.mark.copyright @pytest.mark.linter def test_copyright(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found errors' + rc = main(argv=[".", "test"]) + assert rc == 0, "Found errors" diff --git a/delib_ws_p1/test/test_flake8.py b/delib_ws_p1/test/test_flake8.py index 27ee107..ee79f31 100644 --- a/delib_ws_p1/test/test_flake8.py +++ b/delib_ws_p1/test/test_flake8.py @@ -20,6 +20,6 @@ @pytest.mark.linter def test_flake8(): rc, errors = main_with_errors(argv=[]) - assert rc == 0, \ - 'Found %d code style errors / warnings:\n' % len(errors) + \ - '\n'.join(errors) + assert rc == 0, "Found %d code style errors / warnings:\n" % len( + errors + ) + "\n".join(errors) diff --git a/delib_ws_p1/test/test_pep257.py b/delib_ws_p1/test/test_pep257.py index b234a38..a2c3deb 100644 --- a/delib_ws_p1/test/test_pep257.py +++ b/delib_ws_p1/test/test_pep257.py @@ -19,5 +19,5 @@ @pytest.mark.linter @pytest.mark.pep257 def test_pep257(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found code style errors / warnings' + rc = main(argv=[".", "test"]) + assert rc == 0, "Found code style errors / warnings" diff --git a/delib_ws_p1_py/LICENSE b/delib_ws_p1_py/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/delib_ws_p1_py/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/delib_ws_p1_py/delib_ws_p1_py/__init__.py b/delib_ws_p1_py/delib_ws_p1_py/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/delib_ws_p1_py/delib_ws_p1_py/run.py b/delib_ws_p1_py/delib_ws_p1_py/run.py new file mode 100644 index 0000000..fc34227 --- /dev/null +++ b/delib_ws_p1_py/delib_ws_p1_py/run.py @@ -0,0 +1,31 @@ +import rclpy +from rclpy.action import ActionClient + +from problem_interface.perform_action import PerformAction, ACTIONS +from problem_interface.world_state import WorldState +from delib_ws_p1.is_at_goal import get_goal_state + +import random + +import logging + +import time + +logging.basicConfig(level=logging.INFO) +logger = logging.getLogger("delib_ws_p1_py") + + +def main(): + ws = WorldState() + pa = PerformAction() + assert not ws.get_state( + get_goal_state() + ), "Robot must not be at goal at the beginning" + pa.do_action(ACTIONS.NAVIGATE, "table_source") + pa.do_action(ACTIONS.PICK, "banana0") + pa.do_action(ACTIONS.NAVIGATE, "table_sink") + pa.do_action(ACTIONS.PLACE, "banana0") + assert ws.get_state(get_goal_state()), "Robot must be at goal at the end" + logger.info("Goal reached") + + rclpy.shutdown() diff --git a/delib_ws_p1_py/launch/run.launch.py b/delib_ws_p1_py/launch/run.launch.py new file mode 100644 index 0000000..ebd6a50 --- /dev/null +++ b/delib_ws_p1_py/launch/run.launch.py @@ -0,0 +1,17 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +from ament_index_python.packages import get_package_share_directory + +problem_pkg = get_package_share_directory("delib_ws_p1") + + +def generate_launch_description(): + # Problem node + problem_node = Node(package="delib_ws_p1", executable="run", name="run") + # Solution node + solution_node = Node(package="delib_ws_p1_py", executable="run", name="run") + + return LaunchDescription([problem_node, solution_node]) diff --git a/delib_ws_p1_py/package.xml b/delib_ws_p1_py/package.xml new file mode 100644 index 0000000..d63e26b --- /dev/null +++ b/delib_ws_p1_py/package.xml @@ -0,0 +1,21 @@ + + + + delib_ws_p1_py + 1.0.0 + Solution for problem 1 of the ROSCon24 Deliberation Workshop + Christian Henkel + Apache-2.0 + + rclpy + problem_interface + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/delib_ws_p1_py/resource/delib_ws_p1_py b/delib_ws_p1_py/resource/delib_ws_p1_py new file mode 100644 index 0000000..e69de29 diff --git a/delib_ws_p1_py/setup.cfg b/delib_ws_p1_py/setup.cfg new file mode 100644 index 0000000..b46c86d --- /dev/null +++ b/delib_ws_p1_py/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/delib_ws_p1_py +[install] +install_scripts=$base/lib/delib_ws_p1_py diff --git a/delib_ws_p1_py/setup.py b/delib_ws_p1_py/setup.py new file mode 100644 index 0000000..48fe4f3 --- /dev/null +++ b/delib_ws_p1_py/setup.py @@ -0,0 +1,27 @@ +from setuptools import find_packages, setup + +import os +from glob import glob + +package_name = "delib_ws_p1_py" + +setup( + name=package_name, + version="0.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("launch/*.launch.py")), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="hec2le", + maintainer_email="christian.henkel2@de.bosch.com", + description="TODO: Package description", + license="Apache-2.0", + tests_require=["pytest"], + entry_points={ + "console_scripts": ["run = delib_ws_p1_py.run:main"], + }, +) diff --git a/delib_ws_p1_py/test/test_copyright.py b/delib_ws_p1_py/test/test_copyright.py new file mode 100644 index 0000000..ceffe89 --- /dev/null +++ b/delib_ws_p1_py/test/test_copyright.py @@ -0,0 +1,27 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip( + reason="No copyright header has been placed in the generated source file." +) +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=[".", "test"]) + assert rc == 0, "Found errors" diff --git a/delib_ws_p1_py/test/test_flake8.py b/delib_ws_p1_py/test/test_flake8.py new file mode 100644 index 0000000..ee79f31 --- /dev/null +++ b/delib_ws_p1_py/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, "Found %d code style errors / warnings:\n" % len( + errors + ) + "\n".join(errors) diff --git a/delib_ws_p1_py/test/test_pep257.py b/delib_ws_p1_py/test/test_pep257.py new file mode 100644 index 0000000..a2c3deb --- /dev/null +++ b/delib_ws_p1_py/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=[".", "test"]) + assert rc == 0, "Found code style errors / warnings" diff --git a/delib_ws_p2/data/location_data.yaml b/delib_ws_p2/data/location_data.yaml new file mode 100644 index 0000000..1282330 --- /dev/null +++ b/delib_ws_p2/data/location_data.yaml @@ -0,0 +1,17 @@ +############################# +# Example location metadata # +############################# + +table: + footprint: + type: box + dims: [0.6, 0.6] + height: 0.5 + nav_poses: + - [0, -0.4, 1.57] + locations: + - name: "tabletop" + footprint: + type: parent + padding: 0.1 + color: [0.2, 0.2, 0.2] diff --git a/delib_ws_p2/data/object_data.yaml b/delib_ws_p2/data/object_data.yaml new file mode 100644 index 0000000..2396f0f --- /dev/null +++ b/delib_ws_p2/data/object_data.yaml @@ -0,0 +1,9 @@ +########################### +# Example object metadata # +########################### + +banana: + footprint: + type: box + dims: [0.05, 0.2] + color: [0.7, 0.7, 0] diff --git a/delib_ws_p2/data/world.yaml b/delib_ws_p2/data/world.yaml new file mode 100644 index 0000000..da7b089 --- /dev/null +++ b/delib_ws_p2/data/world.yaml @@ -0,0 +1,57 @@ +metadata: + locations: $PWD/location_data.yaml + objects: $PWD/object_data.yaml + +robots: + - name: robot + radius: 0.1 + location: dining_room + path_executor: + type: constant_velocity + path_planner: + type: rrt + +rooms: + - name: banana_farm + footprint: + type: polygon + coords: + - [-2, 1] + - [-1, 1] + - [-1, -1] + - [-2, -1] + wall_width: 0.2 + color: [1, 0, 0] + - name: dining_room + footprint: + type: polygon + coords: + - [2, 1] + - [1, 1] + - [1, -1] + - [2, -1] + wall_width: 0.2 + color: [0, 1, 0] + +hallways: + - room_start: banana_farm + room_end: dining_room + width: 0.3 + conn_method: auto + is_open: false + is_locked: false + +locations: + - name: table_source + parent: banana_farm + category: table + pose: [-1.5, 0.5] + - name: table_sink + parent: dining_room + category: table + pose: [1.5, 0.5] + +objects: + - parent: table_source + category: banana + pose: [-1.5, 0.5] diff --git a/delib_ws_p2/delib_ws_p2/__init__.py b/delib_ws_p2/delib_ws_p2/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/delib_ws_p2/delib_ws_p2/is_at_goal.py b/delib_ws_p2/delib_ws_p2/is_at_goal.py new file mode 100644 index 0000000..2f180fb --- /dev/null +++ b/delib_ws_p2/delib_ws_p2/is_at_goal.py @@ -0,0 +1,14 @@ +from problem_interface.world_state import WorldState + +import sys + + +def get_goal_state(): + return [("objects.banana0.parent", "table_sink_tabletop")] + + +def main(): + ws = WorldState() + is_at_goal = ws.get_state(get_goal_state()) + print(is_at_goal) + sys.exit(0) diff --git a/delib_ws_p2/delib_ws_p2/run.py b/delib_ws_p2/delib_ws_p2/run.py new file mode 100644 index 0000000..9ba8f16 --- /dev/null +++ b/delib_ws_p2/delib_ws_p2/run.py @@ -0,0 +1,92 @@ +#!/usr/bin/env python3 + +""" +__Goal__: +Banana on table_sink. + +__Initial State__: +Banana on table_source. + +__Available Actions__: + +- Pick object +- Place object +- Move robot + +__Available Conditions__: + +- Robot location + dining_room, banana_farm +- Object location + table_sink, table_source +- Object in hand + true, false + +""" +import os +import rclpy +import threading +import numpy as np + +from pyrobosim.core import Robot, World, WorldYamlLoader +from pyrobosim.gui import start_gui +from pyrobosim.navigation import ConstantVelocityExecutor, PathPlanner +from pyrobosim.utils.pose import Pose +from pyrobosim_ros.ros_interface import WorldROSWrapper +from ament_index_python.packages import get_package_share_directory + + +data_folder = os.path.join(get_package_share_directory("delib_ws_p2"), "data") + + +def create_world(): + """Create a test world""" + # world = World() + world = WorldYamlLoader().from_yaml(os.path.join(data_folder, "world.yaml")) + + # Set the location and object metadata + # world.set_metadata( + # locations=os.path.join(data_folder, "location_data.yaml"), + # objects=os.path.join(data_folder, "object_data.yaml"), + # ) + + return world + + +def create_world_from_yaml(world_file): + return WorldYamlLoader().from_yaml(os.path.join(data_folder, world_file)) + + +def create_ros_node(): + """Initializes ROS node""" + rclpy.init() + node = WorldROSWrapper(state_pub_rate=0.1, dynamics_rate=0.01) + node.declare_parameter("world_file", value="") + + # Set the world + world_file = node.get_parameter("world_file").get_parameter_value().string_value + if world_file == "": + node.get_logger().info("Creating demo world programmatically.") + world = create_world() + else: + node.get_logger().info(f"Using world file {world_file}.") + world = create_world_from_yaml(world_file) + + node.set_world(world) + + return node + + +def main(): + node = create_ros_node() + + # Start ROS node in separate thread + ros_thread = threading.Thread(target=lambda: node.start(wait_for_gui=True)) + ros_thread.start() + + # Start GUI in main thread + start_gui(node.world) + + +if __name__ == "__main__": + main() diff --git a/delib_ws_p2/package.xml b/delib_ws_p2/package.xml new file mode 100644 index 0000000..dc9ddb6 --- /dev/null +++ b/delib_ws_p2/package.xml @@ -0,0 +1,21 @@ + + + + delib_ws_p2 + 1.0.0 + Problem 2 of the ROSCon24 Deliberation Workshop + Christian Henkel + Apache-2.0 + + pyrobosim_ros + problem_interface + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/delib_ws_p2/resource/delib_ws_p2 b/delib_ws_p2/resource/delib_ws_p2 new file mode 100644 index 0000000..e69de29 diff --git a/delib_ws_p2/setup.cfg b/delib_ws_p2/setup.cfg new file mode 100644 index 0000000..ccedfbd --- /dev/null +++ b/delib_ws_p2/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/delib_ws_p2 +[install] +install_scripts=$base/lib/delib_ws_p2 diff --git a/delib_ws_p2/setup.py b/delib_ws_p2/setup.py new file mode 100644 index 0000000..4932c5e --- /dev/null +++ b/delib_ws_p2/setup.py @@ -0,0 +1,29 @@ +from glob import glob + +from setuptools import find_packages, setup + +package_name = "delib_ws_p2" + +setup( + name=package_name, + version="0.0.0", + packages=find_packages(exclude=["test"]), + data_files=[ + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + ("share/" + package_name + "/data", glob("data/*.*")), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="Christian Henkel", + maintainer_email="christian.henkel2@de.bosch.com", + description="TODO: Package description", + license="Apache-2.0", + tests_require=["pytest"], + entry_points={ + "console_scripts": [ + "run = delib_ws_p2.run:main", + "is_at_goal = delib_ws_p2.is_at_goal:main", + ], + }, +) diff --git a/delib_ws_p2/test/test_copyright.py b/delib_ws_p2/test/test_copyright.py new file mode 100644 index 0000000..ceffe89 --- /dev/null +++ b/delib_ws_p2/test/test_copyright.py @@ -0,0 +1,27 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip( + reason="No copyright header has been placed in the generated source file." +) +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=[".", "test"]) + assert rc == 0, "Found errors" diff --git a/delib_ws_p2/test/test_flake8.py b/delib_ws_p2/test/test_flake8.py new file mode 100644 index 0000000..ee79f31 --- /dev/null +++ b/delib_ws_p2/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, "Found %d code style errors / warnings:\n" % len( + errors + ) + "\n".join(errors) diff --git a/delib_ws_p2/test/test_pep257.py b/delib_ws_p2/test/test_pep257.py new file mode 100644 index 0000000..a2c3deb --- /dev/null +++ b/delib_ws_p2/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=[".", "test"]) + assert rc == 0, "Found code style errors / warnings" diff --git a/delib_ws_p2_py/LICENSE b/delib_ws_p2_py/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/delib_ws_p2_py/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/delib_ws_p2_py/delib_ws_p2_py/__init__.py b/delib_ws_p2_py/delib_ws_p2_py/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/delib_ws_p2_py/delib_ws_p2_py/run.py b/delib_ws_p2_py/delib_ws_p2_py/run.py new file mode 100644 index 0000000..a66bf53 --- /dev/null +++ b/delib_ws_p2_py/delib_ws_p2_py/run.py @@ -0,0 +1,33 @@ +import rclpy +from rclpy.action import ActionClient + +from problem_interface.perform_action import PerformAction, ACTIONS +from problem_interface.world_state import WorldState +from delib_ws_p1.is_at_goal import get_goal_state + +import random + +import logging + +import time + +logging.basicConfig(level=logging.INFO) +logger = logging.getLogger("delib_ws_p2_py") + + +def main(): + ws = WorldState() + pa = PerformAction() + assert not ws.get_state( + get_goal_state() + ), "Robot must not be at goal at the beginning" + pa.do_action(ACTIONS.NAVIGATE, "hall_banana_farm_dining_room") + pa.do_action(ACTIONS.OPEN, "hall_banana_farm_dining_room") + pa.do_action(ACTIONS.NAVIGATE, "table_source") + pa.do_action(ACTIONS.PICK, "banana0") + pa.do_action(ACTIONS.NAVIGATE, "table_sink") + pa.do_action(ACTIONS.PLACE, "banana0") + assert ws.get_state(get_goal_state()), "Robot must be at goal at the end" + logger.info("Goal reached") + + rclpy.shutdown() diff --git a/delib_ws_p2_py/launch/run.launch.py b/delib_ws_p2_py/launch/run.launch.py new file mode 100644 index 0000000..07b8e17 --- /dev/null +++ b/delib_ws_p2_py/launch/run.launch.py @@ -0,0 +1,15 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + # Problem node + problem_node = Node(package="delib_ws_p2", executable="run", name="run") + # Solution node + solution_node = Node(package="delib_ws_p2_py", executable="run", name="run") + + return LaunchDescription([problem_node, solution_node]) diff --git a/delib_ws_p2_py/package.xml b/delib_ws_p2_py/package.xml new file mode 100644 index 0000000..48eff54 --- /dev/null +++ b/delib_ws_p2_py/package.xml @@ -0,0 +1,21 @@ + + + + delib_ws_p2_py + 1.0.0 + Solution for problem 1 of the ROSCon24 Deliberation Workshop + Christian Henkel + Apache-2.0 + + rclpy + problem_interface + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/delib_ws_p2_py/resource/delib_ws_p2_py b/delib_ws_p2_py/resource/delib_ws_p2_py new file mode 100644 index 0000000..e69de29 diff --git a/delib_ws_p2_py/setup.cfg b/delib_ws_p2_py/setup.cfg new file mode 100644 index 0000000..ef8d283 --- /dev/null +++ b/delib_ws_p2_py/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/delib_ws_p2_py +[install] +install_scripts=$base/lib/delib_ws_p2_py diff --git a/delib_ws_p2_py/setup.py b/delib_ws_p2_py/setup.py new file mode 100644 index 0000000..77fb5a1 --- /dev/null +++ b/delib_ws_p2_py/setup.py @@ -0,0 +1,27 @@ +from setuptools import find_packages, setup + +import os +from glob import glob + +package_name = "delib_ws_p2_py" + +setup( + name=package_name, + version="0.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("launch/*.launch.py")), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="hec2le", + maintainer_email="christian.henkel2@de.bosch.com", + description="TODO: Package description", + license="Apache-2.0", + tests_require=["pytest"], + entry_points={ + "console_scripts": ["run = delib_ws_p2_py.run:main"], + }, +) diff --git a/delib_ws_p2_py/test/test_copyright.py b/delib_ws_p2_py/test/test_copyright.py new file mode 100644 index 0000000..ceffe89 --- /dev/null +++ b/delib_ws_p2_py/test/test_copyright.py @@ -0,0 +1,27 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip( + reason="No copyright header has been placed in the generated source file." +) +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=[".", "test"]) + assert rc == 0, "Found errors" diff --git a/delib_ws_p2_py/test/test_flake8.py b/delib_ws_p2_py/test/test_flake8.py new file mode 100644 index 0000000..ee79f31 --- /dev/null +++ b/delib_ws_p2_py/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, "Found %d code style errors / warnings:\n" % len( + errors + ) + "\n".join(errors) diff --git a/delib_ws_p2_py/test/test_pep257.py b/delib_ws_p2_py/test/test_pep257.py new file mode 100644 index 0000000..a2c3deb --- /dev/null +++ b/delib_ws_p2_py/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=[".", "test"]) + assert rc == 0, "Found code style errors / warnings" diff --git a/delib_ws_p3/data/location_data.yaml b/delib_ws_p3/data/location_data.yaml new file mode 100644 index 0000000..1282330 --- /dev/null +++ b/delib_ws_p3/data/location_data.yaml @@ -0,0 +1,17 @@ +############################# +# Example location metadata # +############################# + +table: + footprint: + type: box + dims: [0.6, 0.6] + height: 0.5 + nav_poses: + - [0, -0.4, 1.57] + locations: + - name: "tabletop" + footprint: + type: parent + padding: 0.1 + color: [0.2, 0.2, 0.2] diff --git a/delib_ws_p3/data/object_data.yaml b/delib_ws_p3/data/object_data.yaml new file mode 100644 index 0000000..2396f0f --- /dev/null +++ b/delib_ws_p3/data/object_data.yaml @@ -0,0 +1,9 @@ +########################### +# Example object metadata # +########################### + +banana: + footprint: + type: box + dims: [0.05, 0.2] + color: [0.7, 0.7, 0] diff --git a/delib_ws_p3/data/world.yaml b/delib_ws_p3/data/world.yaml new file mode 100644 index 0000000..ba54f2d --- /dev/null +++ b/delib_ws_p3/data/world.yaml @@ -0,0 +1,67 @@ +metadata: + locations: $PWD/location_data.yaml + objects: $PWD/object_data.yaml + +robots: + - name: robot + radius: 0.1 + location: dining_room + path_executor: + type: constant_velocity + path_planner: + type: rrt + action_execution_options: + navigate: + success_probability: 0.9 + rng_seed: 42 + battery_usage: 0 + pick: + success_probability: 0.5 + battery_usage: 0 + place: + battery_usage: 0 + +rooms: + - name: banana_farm + footprint: + type: polygon + coords: + - [-2, 1] + - [-1, 1] + - [-1, -1] + - [-2, -1] + wall_width: 0.2 + color: [1, 0, 0] + - name: dining_room + footprint: + type: polygon + coords: + - [2, 1] + - [1, 1] + - [1, -1] + - [2, -1] + wall_width: 0.2 + color: [0, 1, 0] + +hallways: + - room_start: banana_farm + room_end: dining_room + width: 0.3 + conn_method: auto + is_open: false + is_locked: false + +locations: + - name: table_source + parent: banana_farm + category: table + pose: [-1.5, 0.5] + - name: table_sink + parent: dining_room + category: table + pose: [1.5, 0.5] + +objects: + - parent: table_source + category: banana + pose: [-1.5, 0.5] diff --git a/delib_ws_p3/delib_ws_p3/__init__.py b/delib_ws_p3/delib_ws_p3/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/delib_ws_p3/delib_ws_p3/is_at_goal.py b/delib_ws_p3/delib_ws_p3/is_at_goal.py new file mode 100644 index 0000000..2f180fb --- /dev/null +++ b/delib_ws_p3/delib_ws_p3/is_at_goal.py @@ -0,0 +1,14 @@ +from problem_interface.world_state import WorldState + +import sys + + +def get_goal_state(): + return [("objects.banana0.parent", "table_sink_tabletop")] + + +def main(): + ws = WorldState() + is_at_goal = ws.get_state(get_goal_state()) + print(is_at_goal) + sys.exit(0) diff --git a/delib_ws_p3/delib_ws_p3/run.py b/delib_ws_p3/delib_ws_p3/run.py new file mode 100644 index 0000000..9fca818 --- /dev/null +++ b/delib_ws_p3/delib_ws_p3/run.py @@ -0,0 +1,92 @@ +#!/usr/bin/env python3 + +""" +__Goal__: +Banana on table_sink. + +__Initial State__: +Banana on table_source. + +__Available Actions__: + +- Pick object +- Place object +- Move robot + +__Available Conditions__: + +- Robot location + dining_room, banana_farm +- Object location + table_sink, table_source +- Object in hand + true, false + +""" +import os +import rclpy +import threading +import numpy as np + +from pyrobosim.core import Robot, World, WorldYamlLoader +from pyrobosim.gui import start_gui +from pyrobosim.navigation import ConstantVelocityExecutor, PathPlanner +from pyrobosim.utils.pose import Pose +from pyrobosim_ros.ros_interface import WorldROSWrapper +from ament_index_python.packages import get_package_share_directory + + +data_folder = os.path.join(get_package_share_directory("delib_ws_p3"), "data") + + +def create_world(): + """Create a test world""" + # world = World() + world = WorldYamlLoader().from_yaml(os.path.join(data_folder, "world.yaml")) + + # Set the location and object metadata + world.set_metadata( + locations=os.path.join(data_folder, "location_data.yaml"), + objects=os.path.join(data_folder, "object_data.yaml"), + ) + + return world + + +def create_world_from_yaml(world_file): + return WorldYamlLoader().from_yaml(os.path.join(data_folder, world_file)) + + +def create_ros_node(): + """Initializes ROS node""" + rclpy.init() + node = WorldROSWrapper(state_pub_rate=0.1, dynamics_rate=0.01) + node.declare_parameter("world_file", value="") + + # Set the world + world_file = node.get_parameter("world_file").get_parameter_value().string_value + if world_file == "": + node.get_logger().info("Creating demo world programmatically.") + world = create_world() + else: + node.get_logger().info(f"Using world file {world_file}.") + world = create_world_from_yaml(world_file) + + node.set_world(world) + + return node + + +def main(): + node = create_ros_node() + + # Start ROS node in separate thread + ros_thread = threading.Thread(target=lambda: node.start(wait_for_gui=True)) + ros_thread.start() + + # Start GUI in main thread + start_gui(node.world) + + +if __name__ == "__main__": + main() diff --git a/delib_ws_p3/package.xml b/delib_ws_p3/package.xml new file mode 100644 index 0000000..b7e212f --- /dev/null +++ b/delib_ws_p3/package.xml @@ -0,0 +1,21 @@ + + + + delib_ws_p3 + 1.0.0 + Problem 2 of the ROSCon24 Deliberation Workshop + Christian Henkel + Apache-2.0 + + pyrobosim_ros + problem_interface + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/delib_ws_p3/resource/delib_ws_p3 b/delib_ws_p3/resource/delib_ws_p3 new file mode 100644 index 0000000..e69de29 diff --git a/delib_ws_p3/setup.cfg b/delib_ws_p3/setup.cfg new file mode 100644 index 0000000..1b4625c --- /dev/null +++ b/delib_ws_p3/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/delib_ws_p3 +[install] +install_scripts=$base/lib/delib_ws_p3 diff --git a/delib_ws_p3/setup.py b/delib_ws_p3/setup.py new file mode 100644 index 0000000..42da613 --- /dev/null +++ b/delib_ws_p3/setup.py @@ -0,0 +1,29 @@ +from glob import glob + +from setuptools import find_packages, setup + +package_name = "delib_ws_p3" + +setup( + name=package_name, + version="0.0.0", + packages=find_packages(exclude=["test"]), + data_files=[ + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + ("share/" + package_name + "/data", glob("data/*.*")), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="Christian Henkel", + maintainer_email="christian.henkel2@de.bosch.com", + description="TODO: Package description", + license="Apache-2.0", + tests_require=["pytest"], + entry_points={ + "console_scripts": [ + "run = delib_ws_p3.run:main", + "is_at_goal = delib_ws_p3.is_at_goal:main", + ], + }, +) diff --git a/delib_ws_p3/test/test_copyright.py b/delib_ws_p3/test/test_copyright.py new file mode 100644 index 0000000..ceffe89 --- /dev/null +++ b/delib_ws_p3/test/test_copyright.py @@ -0,0 +1,27 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip( + reason="No copyright header has been placed in the generated source file." +) +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=[".", "test"]) + assert rc == 0, "Found errors" diff --git a/delib_ws_p3/test/test_flake8.py b/delib_ws_p3/test/test_flake8.py new file mode 100644 index 0000000..ee79f31 --- /dev/null +++ b/delib_ws_p3/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, "Found %d code style errors / warnings:\n" % len( + errors + ) + "\n".join(errors) diff --git a/delib_ws_p3/test/test_pep257.py b/delib_ws_p3/test/test_pep257.py new file mode 100644 index 0000000..a2c3deb --- /dev/null +++ b/delib_ws_p3/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=[".", "test"]) + assert rc == 0, "Found code style errors / warnings" diff --git a/delib_ws_p4/data/location_data.yaml b/delib_ws_p4/data/location_data.yaml new file mode 100644 index 0000000..1282330 --- /dev/null +++ b/delib_ws_p4/data/location_data.yaml @@ -0,0 +1,17 @@ +############################# +# Example location metadata # +############################# + +table: + footprint: + type: box + dims: [0.6, 0.6] + height: 0.5 + nav_poses: + - [0, -0.4, 1.57] + locations: + - name: "tabletop" + footprint: + type: parent + padding: 0.1 + color: [0.2, 0.2, 0.2] diff --git a/delib_ws_p4/data/object_data.yaml b/delib_ws_p4/data/object_data.yaml new file mode 100644 index 0000000..2396f0f --- /dev/null +++ b/delib_ws_p4/data/object_data.yaml @@ -0,0 +1,9 @@ +########################### +# Example object metadata # +########################### + +banana: + footprint: + type: box + dims: [0.05, 0.2] + color: [0.7, 0.7, 0] diff --git a/delib_ws_p4/data/world.yaml b/delib_ws_p4/data/world.yaml new file mode 100644 index 0000000..16d1b04 --- /dev/null +++ b/delib_ws_p4/data/world.yaml @@ -0,0 +1,72 @@ +metadata: + locations: $PWD/location_data.yaml + objects: $PWD/object_data.yaml + +robots: + - name: robot + radius: 0.1 + location: dining_room + path_executor: + type: constant_velocity + path_planner: + type: rrt + action_execution_options: + navigate: + success_probability: 0.9 + rng_seed: 42 + battery_usage: 1 + pick: + success_probability: 0.5 + battery_usage: 1 + place: + battery_usage: 1 + +rooms: + - name: banana_farm + footprint: + type: polygon + coords: + - [-2, 1] + - [-1, 1] + - [-1, -1] + - [-2, -1] + wall_width: 0.2 + color: [1, 0, 0] + - name: dining_room + footprint: + type: polygon + coords: + - [2, 1] + - [1, 1] + - [1, -1] + - [2, -1] + wall_width: 0.2 + color: [0, 1, 0] + +hallways: + - room_start: banana_farm + room_end: dining_room + width: 0.3 + conn_method: auto + is_open: false + is_locked: false + +locations: + - name: table_source + parent: banana_farm + category: table + pose: [-1.5, 0.5] + - name: table_sink + parent: dining_room + category: table + pose: [1.5, 0.5] + - name: charging_station + parent: dining_room + category: charging_station + pose: [1.5, -0.5] + is_charger: true + +objects: + - parent: table_source + category: banana + pose: [-1.5, 0.5] diff --git a/delib_ws_p4/delib_ws_p4/__init__.py b/delib_ws_p4/delib_ws_p4/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/delib_ws_p4/delib_ws_p4/is_at_goal.py b/delib_ws_p4/delib_ws_p4/is_at_goal.py new file mode 100644 index 0000000..2f180fb --- /dev/null +++ b/delib_ws_p4/delib_ws_p4/is_at_goal.py @@ -0,0 +1,14 @@ +from problem_interface.world_state import WorldState + +import sys + + +def get_goal_state(): + return [("objects.banana0.parent", "table_sink_tabletop")] + + +def main(): + ws = WorldState() + is_at_goal = ws.get_state(get_goal_state()) + print(is_at_goal) + sys.exit(0) diff --git a/delib_ws_p4/delib_ws_p4/run.py b/delib_ws_p4/delib_ws_p4/run.py new file mode 100644 index 0000000..307fcb6 --- /dev/null +++ b/delib_ws_p4/delib_ws_p4/run.py @@ -0,0 +1,92 @@ +#!/usr/bin/env python3 + +""" +__Goal__: +Banana on table_sink. + +__Initial State__: +Banana on table_source. + +__Available Actions__: + +- Pick object +- Place object +- Move robot + +__Available Conditions__: + +- Robot location + dining_room, banana_farm +- Object location + table_sink, table_source +- Object in hand + true, false + +""" +import os +import rclpy +import threading +import numpy as np + +from pyrobosim.core import Robot, World, WorldYamlLoader +from pyrobosim.gui import start_gui +from pyrobosim.navigation import ConstantVelocityExecutor, PathPlanner +from pyrobosim.utils.pose import Pose +from pyrobosim_ros.ros_interface import WorldROSWrapper +from ament_index_python.packages import get_package_share_directory + + +data_folder = os.path.join(get_package_share_directory("delib_ws_p4"), "data") + + +def create_world(): + """Create a test world""" + # world = World() + world = WorldYamlLoader().from_yaml(os.path.join(data_folder, "world.yaml")) + + # Set the location and object metadata + world.set_metadata( + locations=os.path.join(data_folder, "location_data.yaml"), + objects=os.path.join(data_folder, "object_data.yaml"), + ) + + return world + + +def create_world_from_yaml(world_file): + return WorldYamlLoader().from_yaml(os.path.join(data_folder, world_file)) + + +def create_ros_node(): + """Initializes ROS node""" + rclpy.init() + node = WorldROSWrapper(state_pub_rate=0.1, dynamics_rate=0.01) + node.declare_parameter("world_file", value="") + + # Set the world + world_file = node.get_parameter("world_file").get_parameter_value().string_value + if world_file == "": + node.get_logger().info("Creating demo world programmatically.") + world = create_world() + else: + node.get_logger().info(f"Using world file {world_file}.") + world = create_world_from_yaml(world_file) + + node.set_world(world) + + return node + + +def main(): + node = create_ros_node() + + # Start ROS node in separate thread + ros_thread = threading.Thread(target=lambda: node.start(wait_for_gui=True)) + ros_thread.start() + + # Start GUI in main thread + start_gui(node.world) + + +if __name__ == "__main__": + main() diff --git a/delib_ws_p4/package.xml b/delib_ws_p4/package.xml new file mode 100644 index 0000000..a9e3d5c --- /dev/null +++ b/delib_ws_p4/package.xml @@ -0,0 +1,21 @@ + + + + delib_ws_p4 + 1.0.0 + Problem 2 of the ROSCon24 Deliberation Workshop + Christian Henkel + Apache-2.0 + + pyrobosim_ros + problem_interface + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/delib_ws_p4/resource/delib_ws_p4 b/delib_ws_p4/resource/delib_ws_p4 new file mode 100644 index 0000000..e69de29 diff --git a/delib_ws_p4/setup.cfg b/delib_ws_p4/setup.cfg new file mode 100644 index 0000000..5cd2174 --- /dev/null +++ b/delib_ws_p4/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/delib_ws_p4 +[install] +install_scripts=$base/lib/delib_ws_p4 diff --git a/delib_ws_p4/setup.py b/delib_ws_p4/setup.py new file mode 100644 index 0000000..0e98a5f --- /dev/null +++ b/delib_ws_p4/setup.py @@ -0,0 +1,29 @@ +from glob import glob + +from setuptools import find_packages, setup + +package_name = "delib_ws_p4" + +setup( + name=package_name, + version="0.0.0", + packages=find_packages(exclude=["test"]), + data_files=[ + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + ("share/" + package_name + "/data", glob("data/*.*")), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="Christian Henkel", + maintainer_email="christian.henkel2@de.bosch.com", + description="TODO: Package description", + license="Apache-2.0", + tests_require=["pytest"], + entry_points={ + "console_scripts": [ + "run = delib_ws_p4.run:main", + "is_at_goal = delib_ws_p4.is_at_goal:main", + ], + }, +) diff --git a/delib_ws_p4/test/test_copyright.py b/delib_ws_p4/test/test_copyright.py new file mode 100644 index 0000000..ceffe89 --- /dev/null +++ b/delib_ws_p4/test/test_copyright.py @@ -0,0 +1,27 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip( + reason="No copyright header has been placed in the generated source file." +) +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=[".", "test"]) + assert rc == 0, "Found errors" diff --git a/delib_ws_p4/test/test_flake8.py b/delib_ws_p4/test/test_flake8.py new file mode 100644 index 0000000..ee79f31 --- /dev/null +++ b/delib_ws_p4/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, "Found %d code style errors / warnings:\n" % len( + errors + ) + "\n".join(errors) diff --git a/delib_ws_p4/test/test_pep257.py b/delib_ws_p4/test/test_pep257.py new file mode 100644 index 0000000..a2c3deb --- /dev/null +++ b/delib_ws_p4/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=[".", "test"]) + assert rc == 0, "Found code style errors / warnings" diff --git a/problem_interface/LICENSE b/problem_interface/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/problem_interface/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/problem_interface/package.xml b/problem_interface/package.xml new file mode 100644 index 0000000..70d2a01 --- /dev/null +++ b/problem_interface/package.xml @@ -0,0 +1,21 @@ + + + + problem_interface + 0.0.0 + TODO: Package description + hec2le + Apache-2.0 + + rclpy + pyrobosim_msgs + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/problem_interface/problem_interface/__init__.py b/problem_interface/problem_interface/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/problem_interface/problem_interface/perform_action.py b/problem_interface/problem_interface/perform_action.py new file mode 100644 index 0000000..dd77ae9 --- /dev/null +++ b/problem_interface/problem_interface/perform_action.py @@ -0,0 +1,89 @@ +""" +Python wrapper to perform a high-level action in a PyRoboSim simulation. +""" + +from enum import Enum +import rclpy +from rclpy.action import ActionClient +from typing import List, Optional, Tuple + +from pyrobosim_msgs.action import ExecuteTaskAction +from pyrobosim_msgs.msg import TaskAction + + +ACTIONS = Enum("ACTIONS", "PICK PLACE NAVIGATE OPEN CLOSE DETECT") +ACTION_NAME = "execute_action" + + +class PerformAction: + def __init__(self): + if not rclpy.ok(): + rclpy.init() + self.node = rclpy.create_node("perform_action") + self._action_client = ActionClient(self.node, ExecuteTaskAction, ACTION_NAME) + while not self._action_client.wait_for_server(timeout_sec=1.0): + self.node.get_logger().info( + f"action server {ACTION_NAME} not available, waiting again..." + ) + self.result: Optional[bool] = None + self.expected_state: List[Tuple[str, str]] = [] + self.done = False + self._get_result_future = None + + def do_action(self, command: ACTIONS, target: str) -> None: + """ + Call the service to get the current world state. + + :param command: The command to execute. + available commands: 'pick', 'place', 'navigate', 'open', 'close', 'detect' + :param target: The target of the command. + """ + self.node.get_logger().info(f"Performing action {command} on {target}") + action = TaskAction() + action.robot = "robot" + action.type = command.name.lower() + if command in ( + ACTIONS.PICK, + ACTIONS.PLACE, + ACTIONS.OPEN, + ACTIONS.CLOSE, + ACTIONS.DETECT, + ): + action.object = target + elif command == ACTIONS.NAVIGATE: + action.target_location = target + else: + raise ValueError(f"Unknown command: {command}") + + action_goal = ExecuteTaskAction.Goal() + action_goal.action = action + future = self._action_client.send_goal_async(action_goal) + future.add_done_callback(self.goal_response_callback) + self.done = False + + while rclpy.ok() and not self.done: + rclpy.spin_once(self.node, timeout_sec=0.1) + rclpy.spin_once(self.node, timeout_sec=0.5) + + def goal_response_callback(self, future): + goal_handle = future.result() + if not goal_handle.accepted: + self.node.get_logger().info("Goal rejected :(") + self.done = True + return + + self.node.get_logger().info("Goal accepted :)") + + self._get_result_future = goal_handle.get_result_async() + self._get_result_future.add_done_callback(self.done_callback) + + def done_callback(self, future): + try: + result = future.result().result + status = future.result().status + self.node.get_logger().info( + f"Action finished with result {result} and status {status}" + ) + except Exception as e: + self.node.get_logger().error(f"Exception in done_callback: {e}") + self.done = True diff --git a/problem_interface/problem_interface/world_state.py b/problem_interface/problem_interface/world_state.py new file mode 100644 index 0000000..bb32949 --- /dev/null +++ b/problem_interface/problem_interface/world_state.py @@ -0,0 +1,95 @@ +""" +Python wrapper to get the world state from a PyRoboSim simulation. +""" + +import rclpy +import logging +from typing import List, Optional, Tuple + +from pyrobosim_msgs.srv import RequestWorldState + + +logging.basicConfig(level=logging.INFO) +logger = logging.getLogger(__name__) + + +class WorldState: + def __init__(self): + if not rclpy.ok(): + rclpy.init() + self.node = rclpy.create_node("world_state") + self._client = self.node.create_client( + RequestWorldState, "/request_world_state" + ) + while not self._client.wait_for_service(timeout_sec=1.0): + self.node.get_logger().info("service not available, waiting again...") + self.result: Optional[bool] = None + self.wait: bool = False + self.expected_state: List[Tuple[str, str]] = [] + + def get_state(self, expected_state) -> bool: + """ + Call the service to get the current world state. + + :param expected_state: The expected state of the world. + List of tuples of the form (key, value). + All list elements must be true for the condition to be true. + (e.g. [('objects.banana0.parent', 'table_sink_tabletop')] + tests if the banana is on the table_sink_tabletop) + + """ + self.expected_state = expected_state + request = RequestWorldState.Request() + future = self._client.call_async(request) + future.add_done_callback(self.callback) + self.wait = True + self.result = None + + while rclpy.ok() and self.result is None and self.wait: + rclpy.spin_once(self.node, timeout_sec=0.1) + return self.result + + def callback(self, future): + try: + response = future.result() + # self.node.get_logger().info(f'Response: {response}') + except Exception as e: + self.wait = False + self.node.get_logger().info(f"World state evaluation failed: {e}") + + atomic_results = [] + for key, value in self.expected_state: + obj = response.state + for k in key.split("."): + # logger.info(f'k: {k}') + if isinstance(obj, list): + obj_of_name = [x for x in obj if x.name == k] + assert len(obj_of_name) == 1, ( + f"Expected 1 object with name {k}, got " f"{len(obj_of_name)}" + ) + obj = obj_of_name[0] + else: + assert hasattr(obj, k), f"{obj} has no attribute {k}" + obj = getattr(obj, k) + # logger.info(f'o: {obj}') + atomic_results.append(obj == value) + + self.node.get_logger().info("Atomic results:") + for atomic_result, (key, value) in zip(atomic_results, self.expected_state): + self.node.get_logger().info(f"{key} == {value}: {atomic_result}") + + self.result = all(atomic_results) + if len(atomic_results) > 1: + self.node.get_logger().info(f"Final result: {self.result}") + + +def main(): + node = WorldState() + node.get_state([("objects.banana0.parent", "table_sink_tabletop")]) + + node.node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/problem_interface/resource/problem_interface b/problem_interface/resource/problem_interface new file mode 100644 index 0000000..e69de29 diff --git a/problem_interface/setup.cfg b/problem_interface/setup.cfg new file mode 100644 index 0000000..4d39a5b --- /dev/null +++ b/problem_interface/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/problem_interface +[install] +install_scripts=$base/lib/problem_interface diff --git a/problem_interface/setup.py b/problem_interface/setup.py new file mode 100644 index 0000000..b302871 --- /dev/null +++ b/problem_interface/setup.py @@ -0,0 +1,23 @@ +from setuptools import find_packages, setup + +package_name = "problem_interface" + +setup( + name=package_name, + version="0.0.0", + packages=find_packages(exclude=["test"]), + data_files=[ + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="hec2le", + maintainer_email="christian.henkel2@de.bosch.com", + description="TODO: Package description", + license="Apache-2.0", + tests_require=["pytest"], + entry_points={ + "console_scripts": [], + }, +) diff --git a/problem_interface/test/test_copyright.py b/problem_interface/test/test_copyright.py new file mode 100644 index 0000000..ceffe89 --- /dev/null +++ b/problem_interface/test/test_copyright.py @@ -0,0 +1,27 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip( + reason="No copyright header has been placed in the generated source file." +) +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=[".", "test"]) + assert rc == 0, "Found errors" diff --git a/problem_interface/test/test_flake8.py b/problem_interface/test/test_flake8.py new file mode 100644 index 0000000..ee79f31 --- /dev/null +++ b/problem_interface/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, "Found %d code style errors / warnings:\n" % len( + errors + ) + "\n".join(errors) diff --git a/problem_interface/test/test_pep257.py b/problem_interface/test/test_pep257.py new file mode 100644 index 0000000..a2c3deb --- /dev/null +++ b/problem_interface/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=[".", "test"]) + assert rc == 0, "Found code style errors / warnings" From cbb1bc8ef6f9cab2503a4936f4e40ec9481a8ba2 Mon Sep 17 00:00:00 2001 From: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Date: Tue, 20 Aug 2024 08:09:28 -0400 Subject: [PATCH 16/51] Manage external dependencies with submodules, reuse colcon build artifacts (#8) --------- Co-authored-by: Matthias Mayr --- .colcon/.gitkeep | 0 {docker => .docker}/Dockerfile | 22 +++++++++++----------- .docker/entrypoint.sh | 13 +++++++++++++ .dockerignore | 10 ++++++++++ .github/workflows/lint.yml | 2 +- .github/workflows/ros_test.yml | 6 ++++-- .gitignore | 1 + .gitmodules | 12 ++++++++++++ .pre-commit-config.yaml | 2 ++ README.md | 11 +++++++++-- dependencies/SkiROS2/skiros2 | 1 + dependencies/SkiROS2/skiros2_std_lib | 1 + dependencies/pyrobosim | 1 + docker-compose.yaml | 8 ++++++-- docker/entrypoint.sh | 12 ------------ 15 files changed, 72 insertions(+), 30 deletions(-) create mode 100644 .colcon/.gitkeep rename {docker => .docker}/Dockerfile (66%) create mode 100755 .docker/entrypoint.sh create mode 100644 .dockerignore create mode 100644 .gitmodules create mode 160000 dependencies/SkiROS2/skiros2 create mode 160000 dependencies/SkiROS2/skiros2_std_lib create mode 160000 dependencies/pyrobosim delete mode 100755 docker/entrypoint.sh diff --git a/.colcon/.gitkeep b/.colcon/.gitkeep new file mode 100644 index 0000000..e69de29 diff --git a/docker/Dockerfile b/.docker/Dockerfile similarity index 66% rename from docker/Dockerfile rename to .docker/Dockerfile index 64f153a..f2554bf 100644 --- a/docker/Dockerfile +++ b/.docker/Dockerfile @@ -11,26 +11,26 @@ RUN apt-get install -y \ libegl1 libgl1-mesa-dev libglu1-mesa-dev '^libxcb.*-dev' libx11-xcb-dev \ libxi-dev libxkbcommon-dev libxkbcommon-x11-dev libxrender-dev -# Create a ROS 2 workspace +# Create a ROS 2 workspace. RUN mkdir -p /delib_ws/src/ -# Install pyrobosim and its dependencies -WORKDIR /delib_ws/src -RUN git clone -b main https://github.com/sea-bass/pyrobosim.git +# Install external dependencies. +WORKDIR /delib_ws +COPY dependencies src/dependencies +RUN source /opt/ros/jazzy/setup.bash && \ + rosdep install --from-paths src -y --ignore-src ENV PIP_BREAK_SYSTEM_PACKAGES=1 -RUN cd pyrobosim && \ - pip3 install -e ./pyrobosim && \ - pip3 install -r test/python_test_requirements.txt +RUN cd src/dependencies/pyrobosim && \ + pip3 install -e ./pyrobosim -# Remove MatPlotLib display warnings +# Remove MatPlotLib display warnings. RUN mkdir /tmp/runtime-root ENV XDG_RUNTIME_DIR "/tmp/runtime-root" RUN chmod -R 0700 /tmp/runtime-root ENV NO_AT_BRIDGE 1 -# Set up the entrypoint -WORKDIR /delib_ws +# Set up the entrypoint. CMD /bin/bash -COPY docker/entrypoint.sh /entrypoint.sh +COPY .docker/entrypoint.sh /entrypoint.sh ENTRYPOINT ["/entrypoint.sh"] RUN echo "source /entrypoint.sh" >> ~/.bashrc diff --git a/.docker/entrypoint.sh b/.docker/entrypoint.sh new file mode 100755 index 0000000..e6c3a95 --- /dev/null +++ b/.docker/entrypoint.sh @@ -0,0 +1,13 @@ +#!/bin/bash + +# Source ROS and the deliberation workspace. +source /opt/ros/jazzy/setup.bash +if [ ! -f /delib_ws/install/setup.bash ] +then + # TODO(matthias-mayr): This ignored package is still being ported. + colcon build --symlink-install --packages-ignore skiros2_task +fi +source /delib_ws/install/setup.bash + +# Execute the command passed into this entrypoint. +exec "$@" diff --git a/.dockerignore b/.dockerignore new file mode 100644 index 0000000..9ebcf0d --- /dev/null +++ b/.dockerignore @@ -0,0 +1,10 @@ +.colcon/ +.github/ + +.gitignore +.gitmodules +.pre-commit-config.yaml + +docker-compose.yaml +LICENSE +README.md diff --git a/.github/workflows/lint.yml b/.github/workflows/lint.yml index 5506b14..8b8787d 100644 --- a/.github/workflows/lint.yml +++ b/.github/workflows/lint.yml @@ -11,7 +11,7 @@ jobs: runs-on: ubuntu-latest steps: - name: Check out code - uses: actions/checkout@v2 + uses: actions/checkout@v4 - name: Lint markdown files uses: avto-dev/markdown-lint@v1.5.0 with: diff --git a/.github/workflows/ros_test.yml b/.github/workflows/ros_test.yml index f8f6af8..6411b82 100644 --- a/.github/workflows/ros_test.yml +++ b/.github/workflows/ros_test.yml @@ -10,12 +10,14 @@ on: jobs: build_and_test: - name: ${{ matrix.package }} on ${{ matrix.distro }} + name: Build and test strategy: fail-fast: false runs-on: ubuntu-latest steps: - - uses: actions/checkout@v2 + - uses: actions/checkout@v4 + with: + submodules: true - name: Build and start docker uses: hoverkraft-tech/compose-action@v2.0.1 with: diff --git a/.gitignore b/.gitignore index c18dd8d..cfa608b 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,2 @@ __pycache__/ +.colcon/ diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..5296135 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,12 @@ +[submodule "dependencies/pyrobosim"] + path = dependencies/pyrobosim + url = https://github.com/sea-bass/pyrobosim.git + branch = main +[submodule "dependencies/skiros2"] + path = dependencies/SkiROS2/skiros2 + url = https://github.com/RVMI/skiros2 + branch = fix/ros2_jazzy_build +[submodule "dependencies/skiros2_std_lib"] + path = dependencies/SkiROS2/skiros2_std_lib + url = https://github.com/RVMI/skiros2_std_lib + branch = ros2 diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index a0cf8ff..46d7a9c 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -59,3 +59,5 @@ repos: rev: v3.12.2 hooks: - id: markdown-link-check + +exclude: '(dependencies|.colcon)/.*' diff --git a/README.md b/README.md index 160e448..5d25715 100644 --- a/README.md +++ b/README.md @@ -8,10 +8,10 @@ Deliberation Technologies. ## Setup -First, clone this repository. +First, clone this repository and its submodules. ```bash -git clone -b https://github.com/ros-wg-delib/roscon24-workshop.git +git clone --recurse-submodules https://github.com/ros-wg-delib/roscon24-workshop.git ``` Then, build the Docker image. @@ -32,6 +32,13 @@ Once you're in the container, check that you can run a demo. ros2 run delib_ws_p1 run ``` +**NOTE:** If you want to cleanup the colcon build artifacts across container usage, +you can run this command: + +```bash +sudo rm -rf .colcon/build .colcon/install .colcon/log +``` + ## Problem Descriptions ### Problem 1 diff --git a/dependencies/SkiROS2/skiros2 b/dependencies/SkiROS2/skiros2 new file mode 160000 index 0000000..6b6cc38 --- /dev/null +++ b/dependencies/SkiROS2/skiros2 @@ -0,0 +1 @@ +Subproject commit 6b6cc38df79a99e16ddc2f0b8e9f49fadaa9f284 diff --git a/dependencies/SkiROS2/skiros2_std_lib b/dependencies/SkiROS2/skiros2_std_lib new file mode 160000 index 0000000..be8b027 --- /dev/null +++ b/dependencies/SkiROS2/skiros2_std_lib @@ -0,0 +1 @@ +Subproject commit be8b027c6213936d57d8f6491814dddebb707cf2 diff --git a/dependencies/pyrobosim b/dependencies/pyrobosim new file mode 160000 index 0000000..ea926bb --- /dev/null +++ b/dependencies/pyrobosim @@ -0,0 +1 @@ +Subproject commit ea926bbdf736f5dbc2537dcc0d35789bb54e2d86 diff --git a/docker-compose.yaml b/docker-compose.yaml index 5c5a33b..1eff663 100644 --- a/docker-compose.yaml +++ b/docker-compose.yaml @@ -16,7 +16,7 @@ services: image: roscon_delib_ws_2024:main build: context: . - dockerfile: docker/Dockerfile + dockerfile: .docker/Dockerfile target: roscon_delib_ws_2024 # Ensures signals are actually passed and reaped in the container for shutdowns. # https://docs.docker.com/compose/compose-file/compose-file-v3/#init @@ -34,7 +34,11 @@ services: - NVIDIA_DRIVER_CAPABILITIES=all volumes: # Mount the workshop source code - - ./:/delib_ws/src/workshop_files:rw + - ./:/delib_ws/src/:rw + # Mount the colcon build artifacts + - .colcon/build/:/delib_ws/build/:rw + - .colcon/install/:/delib_ws/install/:rw + - .colcon/log/:/delib_ws/log/:rw # Allows graphical programs in the container - /tmp/.X11-unix:/tmp/.X11-unix:rw - ${XAUTHORITY:-$HOME/.Xauthority}:/root/.Xauthority diff --git a/docker/entrypoint.sh b/docker/entrypoint.sh deleted file mode 100755 index 93ecc2b..0000000 --- a/docker/entrypoint.sh +++ /dev/null @@ -1,12 +0,0 @@ -#!/bin/bash - -# Source ROS and the deliberation workspace -source /opt/ros/jazzy/setup.bash -if [ ! -f /delib_ws/install/setup.bash ] -then - colcon build -fi -source /delib_ws/install/setup.bash - -# Execute the command passed into this entrypoint -exec "$@" From e2c9b5b871f3d5dcb445abbe7a8c1d2223b62f22 Mon Sep 17 00:00:00 2001 From: David Oberacker Date: Tue, 20 Aug 2024 14:14:19 +0200 Subject: [PATCH 17/51] Add ros_bt_py with dependencies. (#9) Signed-off-by: David Oberacker --- .gitmodules | 8 ++++++++ dependencies/ros2_ros_bt_py | 1 + dependencies/rosbridge_suite | 1 + 3 files changed, 10 insertions(+) create mode 160000 dependencies/ros2_ros_bt_py create mode 160000 dependencies/rosbridge_suite diff --git a/.gitmodules b/.gitmodules index 5296135..5928969 100644 --- a/.gitmodules +++ b/.gitmodules @@ -10,3 +10,11 @@ path = dependencies/SkiROS2/skiros2_std_lib url = https://github.com/RVMI/skiros2_std_lib branch = ros2 +[submodule "dependencies/ros2_ros_bt_py"] + path = dependencies/ros2_ros_bt_py + url = https://github.com/fzi-forschungszentrum-informatik/ros2_ros_bt_py.git + branch = dev +[submodule "dependencies/rosbridge_suite"] + path = dependencies/rosbridge_suite + url = https://github.com/RobotWebTools/rosbridge_suite.git + branch = ros2 diff --git a/dependencies/ros2_ros_bt_py b/dependencies/ros2_ros_bt_py new file mode 160000 index 0000000..60cfd02 --- /dev/null +++ b/dependencies/ros2_ros_bt_py @@ -0,0 +1 @@ +Subproject commit 60cfd02a38b67c84f48b01fc6b8f28f8b9ac819b diff --git a/dependencies/rosbridge_suite b/dependencies/rosbridge_suite new file mode 160000 index 0000000..55b8fa3 --- /dev/null +++ b/dependencies/rosbridge_suite @@ -0,0 +1 @@ +Subproject commit 55b8fa321804fdd3b8b1f3e7dd1d987ef73e30f0 From e13b18b9a16bc1e92105ff1e01d85bb4a3ce4553 Mon Sep 17 00:00:00 2001 From: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Date: Tue, 27 Aug 2024 04:41:53 -0400 Subject: [PATCH 18/51] Update to pyrobosim 3.0.0 (#14) --- .docker/Dockerfile | 5 +- delib_ws_p1/data/world.yaml | 58 +++++++++++++++++++ delib_ws_p1/delib_ws_p1/run.py | 85 ++-------------------------- delib_ws_p1_py/delib_ws_p1_py/run.py | 5 -- delib_ws_p1_py/launch/run.launch.py | 2 - delib_ws_p2/data/world.yaml | 1 + delib_ws_p2/delib_ws_p2/run.py | 39 +++---------- delib_ws_p2_py/delib_ws_p2_py/run.py | 5 -- delib_ws_p2_py/launch/run.launch.py | 4 -- delib_ws_p3/data/world.yaml | 1 + delib_ws_p3/delib_ws_p3/run.py | 37 ++---------- delib_ws_p4/data/world.yaml | 1 + delib_ws_p4/delib_ws_p4/run.py | 36 ++---------- dependencies/pyrobosim | 2 +- python-requirements.txt | 11 ++++ 15 files changed, 100 insertions(+), 192 deletions(-) create mode 100644 delib_ws_p1/data/world.yaml create mode 100644 python-requirements.txt diff --git a/.docker/Dockerfile b/.docker/Dockerfile index f2554bf..3fa6e4f 100644 --- a/.docker/Dockerfile +++ b/.docker/Dockerfile @@ -19,9 +19,8 @@ WORKDIR /delib_ws COPY dependencies src/dependencies RUN source /opt/ros/jazzy/setup.bash && \ rosdep install --from-paths src -y --ignore-src -ENV PIP_BREAK_SYSTEM_PACKAGES=1 -RUN cd src/dependencies/pyrobosim && \ - pip3 install -e ./pyrobosim +COPY python-requirements.txt /delib_ws +RUN pip3 install --break-system-packages -r python-requirements.txt # Remove MatPlotLib display warnings. RUN mkdir /tmp/runtime-root diff --git a/delib_ws_p1/data/world.yaml b/delib_ws_p1/data/world.yaml new file mode 100644 index 0000000..1d009b0 --- /dev/null +++ b/delib_ws_p1/data/world.yaml @@ -0,0 +1,58 @@ +metadata: + locations: $PWD/location_data.yaml + objects: $PWD/object_data.yaml + +robots: + - name: robot + radius: 0.1 + location: dining_room + path_executor: + type: constant_velocity + path_planner: + type: rrt + rrt_star: true + +rooms: + - name: banana_farm + footprint: + type: polygon + coords: + - [-2, 1] + - [-1, 1] + - [-1, -1] + - [-2, -1] + wall_width: 0.2 + color: [1, 0, 0] + - name: dining_room + footprint: + type: polygon + coords: + - [2, 1] + - [1, 1] + - [1, -1] + - [2, -1] + wall_width: 0.2 + color: [0, 1, 0] + +hallways: + - room_start: banana_farm + room_end: dining_room + width: 0.3 + conn_method: auto + is_open: true + is_locked: false + +locations: + - name: table_source + parent: banana_farm + category: table + pose: [-1.5, 0.5] + - name: table_sink + parent: dining_room + category: table + pose: [1.5, 0.5] + +objects: + - parent: table_source + category: banana + pose: [-1.5, 0.5] diff --git a/delib_ws_p1/delib_ws_p1/run.py b/delib_ws_p1/delib_ws_p1/run.py index e448c23..f964dad 100644 --- a/delib_ws_p1/delib_ws_p1/run.py +++ b/delib_ws_p1/delib_ws_p1/run.py @@ -26,12 +26,9 @@ import os import rclpy import threading -import numpy as np -from pyrobosim.core import Robot, World, WorldYamlLoader +from pyrobosim.core import WorldYamlLoader from pyrobosim.gui import start_gui -from pyrobosim.navigation import ConstantVelocityExecutor, PathPlanner -from pyrobosim.utils.pose import Pose from pyrobosim_ros.ros_interface import WorldROSWrapper from ament_index_python.packages import get_package_share_directory @@ -39,90 +36,20 @@ data_folder = os.path.join(get_package_share_directory("delib_ws_p1"), "data") -def create_world(): - """Create a test world""" - world = World() - - # Set the location and object metadata - world.set_metadata( - locations=os.path.join(data_folder, "location_data.yaml"), - objects=os.path.join(data_folder, "object_data.yaml"), - ) - - # Add rooms - farm_coords = [(-2, 1), (-1, 1), (-1, -1), (-2, -1)] - world.add_room(name="banana_farm", footprint=farm_coords, color=[1, 0, 0]) - dining_coords = [(2, 1), (1, 1), (1, -1), (2, -1)] - world.add_room(name="dining_room", footprint=dining_coords, color=[0, 1, 0]) - - # Add hallways between the rooms - world.add_hallway(room_start="banana_farm", room_end="dining_room", width=0.3) - - # Add locations - table_source = world.add_location( - category="table", - parent="banana_farm", - name="table_source", - pose=Pose(x=-1.5, y=0.5), +def create_world_from_yaml(): + world_file = os.path.join( + get_package_share_directory("delib_ws_p1"), "data", "world.yaml" ) - - table = world.add_location( - category="table", - parent="dining_room", - name="table_sink", - pose=Pose(x=1.5, y=0.5), - ) - - # Add objects - world.add_object( - category="banana", - parent=table_source, - pose=Pose(x=-1.5, y=0.5, yaw=np.pi / 4.0), - ) - - # Add a robot - # Create path planner - planner_config = { - "world": world, - "bidirectional": True, - "rrt_connect": False, - "rrt_star": True, - "collision_check_step_dist": 0.025, - "max_connection_dist": 0.5, - "rewire_radius": 1.5, - "compress_path": False, - } - path_planner = PathPlanner("rrt", **planner_config) - robot = Robot( - name="robot", - radius=0.1, - path_executor=ConstantVelocityExecutor(), - path_planner=path_planner, - ) - world.add_robot(robot, loc="dining_room") - - return world - - -def create_world_from_yaml(world_file): - return WorldYamlLoader().from_yaml(os.path.join(data_folder, world_file)) + return WorldYamlLoader().from_yaml(world_file) def create_ros_node(): """Initializes ROS node""" rclpy.init() node = WorldROSWrapper(state_pub_rate=0.1, dynamics_rate=0.01) - node.declare_parameter("world_file", value="") # Set the world - world_file = node.get_parameter("world_file").get_parameter_value().string_value - if world_file == "": - node.get_logger().info("Creating demo world programmatically.") - world = create_world() - else: - node.get_logger().info(f"Using world file {world_file}.") - world = create_world_from_yaml(world_file) - + world = create_world_from_yaml() node.set_world(world) return node diff --git a/delib_ws_p1_py/delib_ws_p1_py/run.py b/delib_ws_p1_py/delib_ws_p1_py/run.py index fc34227..faf21b6 100644 --- a/delib_ws_p1_py/delib_ws_p1_py/run.py +++ b/delib_ws_p1_py/delib_ws_p1_py/run.py @@ -1,16 +1,11 @@ import rclpy -from rclpy.action import ActionClient from problem_interface.perform_action import PerformAction, ACTIONS from problem_interface.world_state import WorldState from delib_ws_p1.is_at_goal import get_goal_state -import random - import logging -import time - logging.basicConfig(level=logging.INFO) logger = logging.getLogger("delib_ws_p1_py") diff --git a/delib_ws_p1_py/launch/run.launch.py b/delib_ws_p1_py/launch/run.launch.py index ebd6a50..1684512 100644 --- a/delib_ws_p1_py/launch/run.launch.py +++ b/delib_ws_p1_py/launch/run.launch.py @@ -1,6 +1,4 @@ from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node from ament_index_python.packages import get_package_share_directory diff --git a/delib_ws_p2/data/world.yaml b/delib_ws_p2/data/world.yaml index da7b089..8bd281f 100644 --- a/delib_ws_p2/data/world.yaml +++ b/delib_ws_p2/data/world.yaml @@ -10,6 +10,7 @@ robots: type: constant_velocity path_planner: type: rrt + rrt_star: true rooms: - name: banana_farm diff --git a/delib_ws_p2/delib_ws_p2/run.py b/delib_ws_p2/delib_ws_p2/run.py index 9ba8f16..d67df90 100644 --- a/delib_ws_p2/delib_ws_p2/run.py +++ b/delib_ws_p2/delib_ws_p2/run.py @@ -26,52 +26,27 @@ import os import rclpy import threading -import numpy as np -from pyrobosim.core import Robot, World, WorldYamlLoader +from pyrobosim.core import WorldYamlLoader from pyrobosim.gui import start_gui -from pyrobosim.navigation import ConstantVelocityExecutor, PathPlanner -from pyrobosim.utils.pose import Pose from pyrobosim_ros.ros_interface import WorldROSWrapper from ament_index_python.packages import get_package_share_directory -data_folder = os.path.join(get_package_share_directory("delib_ws_p2"), "data") - - -def create_world(): - """Create a test world""" - # world = World() - world = WorldYamlLoader().from_yaml(os.path.join(data_folder, "world.yaml")) - - # Set the location and object metadata - # world.set_metadata( - # locations=os.path.join(data_folder, "location_data.yaml"), - # objects=os.path.join(data_folder, "object_data.yaml"), - # ) - - return world - - -def create_world_from_yaml(world_file): - return WorldYamlLoader().from_yaml(os.path.join(data_folder, world_file)) +def create_world_from_yaml(): + world_file = os.path.join( + get_package_share_directory("delib_ws_p2"), "data", "world.yaml" + ) + return WorldYamlLoader().from_yaml(world_file) def create_ros_node(): """Initializes ROS node""" rclpy.init() node = WorldROSWrapper(state_pub_rate=0.1, dynamics_rate=0.01) - node.declare_parameter("world_file", value="") # Set the world - world_file = node.get_parameter("world_file").get_parameter_value().string_value - if world_file == "": - node.get_logger().info("Creating demo world programmatically.") - world = create_world() - else: - node.get_logger().info(f"Using world file {world_file}.") - world = create_world_from_yaml(world_file) - + world = create_world_from_yaml() node.set_world(world) return node diff --git a/delib_ws_p2_py/delib_ws_p2_py/run.py b/delib_ws_p2_py/delib_ws_p2_py/run.py index a66bf53..8e0b43a 100644 --- a/delib_ws_p2_py/delib_ws_p2_py/run.py +++ b/delib_ws_p2_py/delib_ws_p2_py/run.py @@ -1,16 +1,11 @@ import rclpy -from rclpy.action import ActionClient from problem_interface.perform_action import PerformAction, ACTIONS from problem_interface.world_state import WorldState from delib_ws_p1.is_at_goal import get_goal_state -import random - import logging -import time - logging.basicConfig(level=logging.INFO) logger = logging.getLogger("delib_ws_p2_py") diff --git a/delib_ws_p2_py/launch/run.launch.py b/delib_ws_p2_py/launch/run.launch.py index 07b8e17..01ba174 100644 --- a/delib_ws_p2_py/launch/run.launch.py +++ b/delib_ws_p2_py/launch/run.launch.py @@ -1,10 +1,6 @@ from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node -from ament_index_python.packages import get_package_share_directory - def generate_launch_description(): # Problem node diff --git a/delib_ws_p3/data/world.yaml b/delib_ws_p3/data/world.yaml index ba54f2d..0dc608d 100644 --- a/delib_ws_p3/data/world.yaml +++ b/delib_ws_p3/data/world.yaml @@ -10,6 +10,7 @@ robots: type: constant_velocity path_planner: type: rrt + rrt_star: true action_execution_options: navigate: success_probability: 0.9 diff --git a/delib_ws_p3/delib_ws_p3/run.py b/delib_ws_p3/delib_ws_p3/run.py index 9fca818..21fc7ac 100644 --- a/delib_ws_p3/delib_ws_p3/run.py +++ b/delib_ws_p3/delib_ws_p3/run.py @@ -26,52 +26,27 @@ import os import rclpy import threading -import numpy as np -from pyrobosim.core import Robot, World, WorldYamlLoader +from pyrobosim.core import WorldYamlLoader from pyrobosim.gui import start_gui -from pyrobosim.navigation import ConstantVelocityExecutor, PathPlanner -from pyrobosim.utils.pose import Pose from pyrobosim_ros.ros_interface import WorldROSWrapper from ament_index_python.packages import get_package_share_directory -data_folder = os.path.join(get_package_share_directory("delib_ws_p3"), "data") - - -def create_world(): - """Create a test world""" - # world = World() - world = WorldYamlLoader().from_yaml(os.path.join(data_folder, "world.yaml")) - - # Set the location and object metadata - world.set_metadata( - locations=os.path.join(data_folder, "location_data.yaml"), - objects=os.path.join(data_folder, "object_data.yaml"), +def create_world_from_yaml(): + world_file = os.path.join( + get_package_share_directory("delib_ws_p3"), "data", "world.yaml" ) - - return world - - -def create_world_from_yaml(world_file): - return WorldYamlLoader().from_yaml(os.path.join(data_folder, world_file)) + return WorldYamlLoader().from_yaml(world_file) def create_ros_node(): """Initializes ROS node""" rclpy.init() node = WorldROSWrapper(state_pub_rate=0.1, dynamics_rate=0.01) - node.declare_parameter("world_file", value="") # Set the world - world_file = node.get_parameter("world_file").get_parameter_value().string_value - if world_file == "": - node.get_logger().info("Creating demo world programmatically.") - world = create_world() - else: - node.get_logger().info(f"Using world file {world_file}.") - world = create_world_from_yaml(world_file) - + world = create_world_from_yaml() node.set_world(world) return node diff --git a/delib_ws_p4/data/world.yaml b/delib_ws_p4/data/world.yaml index 16d1b04..99a517d 100644 --- a/delib_ws_p4/data/world.yaml +++ b/delib_ws_p4/data/world.yaml @@ -10,6 +10,7 @@ robots: type: constant_velocity path_planner: type: rrt + rrt_star: true action_execution_options: navigate: success_probability: 0.9 diff --git a/delib_ws_p4/delib_ws_p4/run.py b/delib_ws_p4/delib_ws_p4/run.py index 307fcb6..4adc96b 100644 --- a/delib_ws_p4/delib_ws_p4/run.py +++ b/delib_ws_p4/delib_ws_p4/run.py @@ -28,50 +28,26 @@ import threading import numpy as np -from pyrobosim.core import Robot, World, WorldYamlLoader +from pyrobosim.core import WorldYamlLoader from pyrobosim.gui import start_gui -from pyrobosim.navigation import ConstantVelocityExecutor, PathPlanner -from pyrobosim.utils.pose import Pose from pyrobosim_ros.ros_interface import WorldROSWrapper from ament_index_python.packages import get_package_share_directory -data_folder = os.path.join(get_package_share_directory("delib_ws_p4"), "data") - - -def create_world(): - """Create a test world""" - # world = World() - world = WorldYamlLoader().from_yaml(os.path.join(data_folder, "world.yaml")) - - # Set the location and object metadata - world.set_metadata( - locations=os.path.join(data_folder, "location_data.yaml"), - objects=os.path.join(data_folder, "object_data.yaml"), +def create_world_from_yaml(): + world_file = os.path.join( + get_package_share_directory("delib_ws_p4"), "data", "world.yaml" ) - - return world - - -def create_world_from_yaml(world_file): - return WorldYamlLoader().from_yaml(os.path.join(data_folder, world_file)) + return WorldYamlLoader().from_yaml(world_file) def create_ros_node(): """Initializes ROS node""" rclpy.init() node = WorldROSWrapper(state_pub_rate=0.1, dynamics_rate=0.01) - node.declare_parameter("world_file", value="") # Set the world - world_file = node.get_parameter("world_file").get_parameter_value().string_value - if world_file == "": - node.get_logger().info("Creating demo world programmatically.") - world = create_world() - else: - node.get_logger().info(f"Using world file {world_file}.") - world = create_world_from_yaml(world_file) - + world = create_world_from_yaml() node.set_world(world) return node diff --git a/dependencies/pyrobosim b/dependencies/pyrobosim index ea926bb..549c144 160000 --- a/dependencies/pyrobosim +++ b/dependencies/pyrobosim @@ -1 +1 @@ -Subproject commit ea926bbdf736f5dbc2537dcc0d35789bb54e2d86 +Subproject commit 549c144a029e1fecdea0970d118021355c3cbe70 diff --git a/python-requirements.txt b/python-requirements.txt new file mode 100644 index 0000000..48cf852 --- /dev/null +++ b/python-requirements.txt @@ -0,0 +1,11 @@ +adjustText +astar +matplotlib +numpy<2.1.0 +pycollada +PySide6>=6.4.0 +PyYAML +scipy +shapely>=2.0.1 +transforms3d +trimesh From 2b60c18ec526b14b74fb3a69056d91f6e631c90c Mon Sep 17 00:00:00 2001 From: Matthias Mayr Date: Thu, 29 Aug 2024 09:02:14 +0200 Subject: [PATCH 19/51] Adds SkiROS2 skill repo (#10) * New: Adds repo for SkiROS2 implementation * Fix: Corrects SkiROS2 submodules and add deps * Consolidate apt installs * New: Adds SkiROS2 task planner installation --------- Co-authored-by: Sebastian Castro --- .docker/Dockerfile | 10 +++++++--- .gitmodules | 8 ++++++-- dependencies/SkiROS2/skiros2_pyrobosim_lib | 1 + 3 files changed, 14 insertions(+), 5 deletions(-) create mode 160000 dependencies/SkiROS2/skiros2_pyrobosim_lib diff --git a/.docker/Dockerfile b/.docker/Dockerfile index 3fa6e4f..91fb574 100644 --- a/.docker/Dockerfile +++ b/.docker/Dockerfile @@ -5,9 +5,10 @@ SHELL ["/bin/bash", "-o", "pipefail", "-c"] # Install dependencies ARG DEBIAN_FRONTEND=noninteractive -RUN apt-get update && apt-get upgrade -y -RUN apt-get install -y \ - apt-utils python3-pip python3-tk \ +RUN apt-get update && \ + apt-get upgrade -y && \ + apt-get install -y \ + apt-utils python3-pip python3-tk python3-wrapt python3-inflection \ libegl1 libgl1-mesa-dev libglu1-mesa-dev '^libxcb.*-dev' libx11-xcb-dev \ libxi-dev libxkbcommon-dev libxkbcommon-x11-dev libxrender-dev @@ -19,8 +20,11 @@ WORKDIR /delib_ws COPY dependencies src/dependencies RUN source /opt/ros/jazzy/setup.bash && \ rosdep install --from-paths src -y --ignore-src +# Aggregated Python dependencies COPY python-requirements.txt /delib_ws RUN pip3 install --break-system-packages -r python-requirements.txt +# SkiROS2 dependencies +RUN src/dependencies/SkiROS2/skiros2/skiros2/scripts/install_fd_task_planner.sh # Remove MatPlotLib display warnings. RUN mkdir /tmp/runtime-root diff --git a/.gitmodules b/.gitmodules index 5928969..690d18f 100644 --- a/.gitmodules +++ b/.gitmodules @@ -2,11 +2,11 @@ path = dependencies/pyrobosim url = https://github.com/sea-bass/pyrobosim.git branch = main -[submodule "dependencies/skiros2"] +[submodule "dependencies/SkiROS2/skiros2"] path = dependencies/SkiROS2/skiros2 url = https://github.com/RVMI/skiros2 branch = fix/ros2_jazzy_build -[submodule "dependencies/skiros2_std_lib"] +[submodule "dependencies/SkiROS2/skiros2_std_lib"] path = dependencies/SkiROS2/skiros2_std_lib url = https://github.com/RVMI/skiros2_std_lib branch = ros2 @@ -18,3 +18,7 @@ path = dependencies/rosbridge_suite url = https://github.com/RobotWebTools/rosbridge_suite.git branch = ros2 +[submodule "dependencies/SkiROS2/skiros2_pyrobosim_lib"] + path = dependencies/SkiROS2/skiros2_pyrobosim_lib + url = https://github.com/matthias-mayr/skiros2_pyrobosim_lib.git + branch = main diff --git a/dependencies/SkiROS2/skiros2_pyrobosim_lib b/dependencies/SkiROS2/skiros2_pyrobosim_lib new file mode 160000 index 0000000..ed5336b --- /dev/null +++ b/dependencies/SkiROS2/skiros2_pyrobosim_lib @@ -0,0 +1 @@ +Subproject commit ed5336b3aff9f85a20deb6b5dfac1537b337bd7a From 47eca62f4d3c5f79a83b6bc7b882f703ba62caf6 Mon Sep 17 00:00:00 2001 From: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Date: Mon, 2 Sep 2024 12:14:59 -0400 Subject: [PATCH 20/51] Simplify Python packages (#18) --- README.md | 16 +- delib_ws_p1/delib_ws_p1/is_at_goal.py | 14 -- delib_ws_p1/delib_ws_p1/run.py | 70 ------ delib_ws_p1/package.xml | 21 -- delib_ws_p1/setup.cfg | 4 - delib_ws_p1/setup.py | 29 --- delib_ws_p1_py/delib_ws_p1_py/run.py | 26 --- delib_ws_p1_py/launch/run.launch.py | 15 -- delib_ws_p1_py/package.xml | 21 -- delib_ws_p1_py/setup.cfg | 4 - delib_ws_p2/data/location_data.yaml | 17 -- delib_ws_p2/data/object_data.yaml | 9 - delib_ws_p2/delib_ws_p2/is_at_goal.py | 14 -- delib_ws_p2/package.xml | 21 -- delib_ws_p2/setup.cfg | 4 - delib_ws_p2/setup.py | 29 --- delib_ws_p2_py/delib_ws_p2_py/__init__.py | 0 delib_ws_p2_py/delib_ws_p2_py/run.py | 28 --- delib_ws_p2_py/launch/run.launch.py | 11 - delib_ws_p2_py/resource/delib_ws_p2_py | 0 delib_ws_p2_py/setup.cfg | 4 - delib_ws_p2_py/setup.py | 27 --- delib_ws_p2_py/test/test_copyright.py | 27 --- delib_ws_p2_py/test/test_flake8.py | 25 --- delib_ws_p2_py/test/test_pep257.py | 23 -- delib_ws_p3/data/location_data.yaml | 17 -- delib_ws_p3/data/object_data.yaml | 9 - delib_ws_p3/delib_ws_p3/__init__.py | 0 delib_ws_p3/delib_ws_p3/is_at_goal.py | 14 -- delib_ws_p3/delib_ws_p3/run.py | 67 ------ delib_ws_p3/resource/delib_ws_p3 | 0 delib_ws_p3/setup.cfg | 4 - delib_ws_p3/test/test_copyright.py | 27 --- delib_ws_p3/test/test_flake8.py | 25 --- delib_ws_p3/test/test_pep257.py | 23 -- delib_ws_p4/data/location_data.yaml | 17 -- delib_ws_p4/data/object_data.yaml | 9 - delib_ws_p4/delib_ws_p4/__init__.py | 0 delib_ws_p4/delib_ws_p4/is_at_goal.py | 14 -- delib_ws_p4/delib_ws_p4/run.py | 68 ------ delib_ws_p4/package.xml | 21 -- delib_ws_p4/resource/delib_ws_p4 | 0 delib_ws_p4/setup.cfg | 4 - delib_ws_p4/setup.py | 29 --- delib_ws_p4/test/test_copyright.py | 27 --- delib_ws_p4/test/test_flake8.py | 25 --- delib_ws_p4/test/test_pep257.py | 23 -- .../LICENSE | 0 .../delib_ws_problem_interface}/__init__.py | 0 .../perform_action.py | 0 .../world_state.py | 11 +- .../package.xml | 6 +- .../resource/delib_ws_problem_interface | 0 delib_ws_problem_interface/setup.cfg | 4 + .../setup.py | 6 +- .../test/test_copyright.py | 0 .../test/test_flake8.py | 0 .../test/test_pep257.py | 0 {delib_ws_p2_py => delib_ws_python}/LICENSE | 0 .../delib_ws_python}/__init__.py | 0 delib_ws_python/delib_ws_python/run.py | 48 +++++ delib_ws_python/delib_ws_python/solutions.py | 37 ++++ delib_ws_python/launch/run.launch.py | 25 +++ .../package.xml | 6 +- .../resource/delib_ws_python | 0 delib_ws_python/setup.cfg | 4 + {delib_ws_p1_py => delib_ws_python}/setup.py | 6 +- .../test/test_copyright.py | 0 .../test/test_flake8.py | 0 .../test/test_pep257.py | 0 .../delib_ws_worlds}/__init__.py | 0 delib_ws_worlds/delib_ws_worlds/is_at_goal.py | 23 ++ .../delib_ws_worlds}/run.py | 42 ++-- {delib_ws_p3 => delib_ws_worlds}/package.xml | 6 +- .../resource/delib_ws_worlds | 0 delib_ws_worlds/setup.cfg | 4 + {delib_ws_p3 => delib_ws_worlds}/setup.py | 12 +- .../test/test_copyright.py | 0 .../test/test_flake8.py | 0 .../test/test_pep257.py | 0 .../worlds}/location_data.yaml | 0 .../worlds}/object_data.yaml | 0 .../worlds/world1.yaml | 21 ++ .../worlds/world2.yaml | 39 ++-- .../worlds/world3.yaml | 24 +++ .../worlds/world4.yaml | 44 ++++ problem_interface/LICENSE | 202 ------------------ .../problem_interface/__init__.py | 0 problem_interface/resource/problem_interface | 0 problem_interface/setup.cfg | 4 - problem_interface/test/test_copyright.py | 27 --- problem_interface/test/test_flake8.py | 25 --- problem_interface/test/test_pep257.py | 23 -- 93 files changed, 312 insertions(+), 1219 deletions(-) delete mode 100644 delib_ws_p1/delib_ws_p1/is_at_goal.py delete mode 100644 delib_ws_p1/delib_ws_p1/run.py delete mode 100644 delib_ws_p1/package.xml delete mode 100644 delib_ws_p1/setup.cfg delete mode 100644 delib_ws_p1/setup.py delete mode 100644 delib_ws_p1_py/delib_ws_p1_py/run.py delete mode 100644 delib_ws_p1_py/launch/run.launch.py delete mode 100644 delib_ws_p1_py/package.xml delete mode 100644 delib_ws_p1_py/setup.cfg delete mode 100644 delib_ws_p2/data/location_data.yaml delete mode 100644 delib_ws_p2/data/object_data.yaml delete mode 100644 delib_ws_p2/delib_ws_p2/is_at_goal.py delete mode 100644 delib_ws_p2/package.xml delete mode 100644 delib_ws_p2/setup.cfg delete mode 100644 delib_ws_p2/setup.py delete mode 100644 delib_ws_p2_py/delib_ws_p2_py/__init__.py delete mode 100644 delib_ws_p2_py/delib_ws_p2_py/run.py delete mode 100644 delib_ws_p2_py/launch/run.launch.py delete mode 100644 delib_ws_p2_py/resource/delib_ws_p2_py delete mode 100644 delib_ws_p2_py/setup.cfg delete mode 100644 delib_ws_p2_py/setup.py delete mode 100644 delib_ws_p2_py/test/test_copyright.py delete mode 100644 delib_ws_p2_py/test/test_flake8.py delete mode 100644 delib_ws_p2_py/test/test_pep257.py delete mode 100644 delib_ws_p3/data/location_data.yaml delete mode 100644 delib_ws_p3/data/object_data.yaml delete mode 100644 delib_ws_p3/delib_ws_p3/__init__.py delete mode 100644 delib_ws_p3/delib_ws_p3/is_at_goal.py delete mode 100644 delib_ws_p3/delib_ws_p3/run.py delete mode 100644 delib_ws_p3/resource/delib_ws_p3 delete mode 100644 delib_ws_p3/setup.cfg delete mode 100644 delib_ws_p3/test/test_copyright.py delete mode 100644 delib_ws_p3/test/test_flake8.py delete mode 100644 delib_ws_p3/test/test_pep257.py delete mode 100644 delib_ws_p4/data/location_data.yaml delete mode 100644 delib_ws_p4/data/object_data.yaml delete mode 100644 delib_ws_p4/delib_ws_p4/__init__.py delete mode 100644 delib_ws_p4/delib_ws_p4/is_at_goal.py delete mode 100644 delib_ws_p4/delib_ws_p4/run.py delete mode 100644 delib_ws_p4/package.xml delete mode 100644 delib_ws_p4/resource/delib_ws_p4 delete mode 100644 delib_ws_p4/setup.cfg delete mode 100644 delib_ws_p4/setup.py delete mode 100644 delib_ws_p4/test/test_copyright.py delete mode 100644 delib_ws_p4/test/test_flake8.py delete mode 100644 delib_ws_p4/test/test_pep257.py rename {delib_ws_p1_py => delib_ws_problem_interface}/LICENSE (100%) rename {delib_ws_p1/delib_ws_p1 => delib_ws_problem_interface/delib_ws_problem_interface}/__init__.py (100%) rename {problem_interface/problem_interface => delib_ws_problem_interface/delib_ws_problem_interface}/perform_action.py (100%) rename {problem_interface/problem_interface => delib_ws_problem_interface/delib_ws_problem_interface}/world_state.py (88%) rename {problem_interface => delib_ws_problem_interface}/package.xml (79%) rename delib_ws_p1/resource/delib_ws_p1 => delib_ws_problem_interface/resource/delib_ws_problem_interface (100%) create mode 100644 delib_ws_problem_interface/setup.cfg rename {problem_interface => delib_ws_problem_interface}/setup.py (79%) rename {delib_ws_p1 => delib_ws_problem_interface}/test/test_copyright.py (100%) rename {delib_ws_p1 => delib_ws_problem_interface}/test/test_flake8.py (100%) rename {delib_ws_p1 => delib_ws_problem_interface}/test/test_pep257.py (100%) rename {delib_ws_p2_py => delib_ws_python}/LICENSE (100%) rename {delib_ws_p1_py/delib_ws_p1_py => delib_ws_python/delib_ws_python}/__init__.py (100%) create mode 100644 delib_ws_python/delib_ws_python/run.py create mode 100644 delib_ws_python/delib_ws_python/solutions.py create mode 100644 delib_ws_python/launch/run.launch.py rename {delib_ws_p2_py => delib_ws_python}/package.xml (78%) rename delib_ws_p1_py/resource/delib_ws_p1_py => delib_ws_python/resource/delib_ws_python (100%) create mode 100644 delib_ws_python/setup.cfg rename {delib_ws_p1_py => delib_ws_python}/setup.py (79%) rename {delib_ws_p1_py => delib_ws_python}/test/test_copyright.py (100%) rename {delib_ws_p1_py => delib_ws_python}/test/test_flake8.py (100%) rename {delib_ws_p1_py => delib_ws_python}/test/test_pep257.py (100%) rename {delib_ws_p2/delib_ws_p2 => delib_ws_worlds/delib_ws_worlds}/__init__.py (100%) create mode 100644 delib_ws_worlds/delib_ws_worlds/is_at_goal.py rename {delib_ws_p2/delib_ws_p2 => delib_ws_worlds/delib_ws_worlds}/run.py (60%) rename {delib_ws_p3 => delib_ws_worlds}/package.xml (78%) rename delib_ws_p2/resource/delib_ws_p2 => delib_ws_worlds/resource/delib_ws_worlds (100%) create mode 100644 delib_ws_worlds/setup.cfg rename {delib_ws_p3 => delib_ws_worlds}/setup.py (65%) rename {delib_ws_p2 => delib_ws_worlds}/test/test_copyright.py (100%) rename {delib_ws_p2 => delib_ws_worlds}/test/test_flake8.py (100%) rename {delib_ws_p2 => delib_ws_worlds}/test/test_pep257.py (100%) rename {delib_ws_p1/data => delib_ws_worlds/worlds}/location_data.yaml (100%) rename {delib_ws_p1/data => delib_ws_worlds/worlds}/object_data.yaml (100%) rename delib_ws_p1/data/world.yaml => delib_ws_worlds/worlds/world1.yaml (74%) rename delib_ws_p4/data/world.yaml => delib_ws_worlds/worlds/world2.yaml (72%) rename delib_ws_p3/data/world.yaml => delib_ws_worlds/worlds/world3.yaml (76%) rename delib_ws_p2/data/world.yaml => delib_ws_worlds/worlds/world4.yaml (54%) delete mode 100644 problem_interface/LICENSE delete mode 100644 problem_interface/problem_interface/__init__.py delete mode 100644 problem_interface/resource/problem_interface delete mode 100644 problem_interface/setup.cfg delete mode 100644 problem_interface/test/test_copyright.py delete mode 100644 problem_interface/test/test_flake8.py delete mode 100644 problem_interface/test/test_pep257.py diff --git a/README.md b/README.md index 5d25715..1e841b1 100644 --- a/README.md +++ b/README.md @@ -43,7 +43,9 @@ sudo rm -rf .colcon/build .colcon/install .colcon/log ### Problem 1 -[package delib_ws_p1](delib_ws_p1) +```bash +ros2 run delib_ws_worlds run --ros-args -p problem_number:=1 +``` __Goal__: Banana on table 2. @@ -68,7 +70,9 @@ __Available Conditions__: ### Problem 2 -[package delib_ws_p2](delib_ws_p2) +```bash +ros2 run delib_ws_worlds run --ros-args -p problem_number:=2 +``` __Goal__: Banana on table 2. @@ -98,7 +102,9 @@ __Available Conditions__: ### Problem 3 -[package delib_ws_p3](delib_ws_p3) +```bash +ros2 run delib_ws_worlds run --ros-args -p problem_number:=3 +``` __Goal__: Banana on table 2. @@ -132,7 +138,9 @@ __Available Conditions__: ### Problem 4 -[package delib_ws_p4](delib_ws_p4) +```bash +ros2 run delib_ws_worlds run --ros-args -p problem_number:=4 +``` __Goal__: Banana on table 2. diff --git a/delib_ws_p1/delib_ws_p1/is_at_goal.py b/delib_ws_p1/delib_ws_p1/is_at_goal.py deleted file mode 100644 index 2f180fb..0000000 --- a/delib_ws_p1/delib_ws_p1/is_at_goal.py +++ /dev/null @@ -1,14 +0,0 @@ -from problem_interface.world_state import WorldState - -import sys - - -def get_goal_state(): - return [("objects.banana0.parent", "table_sink_tabletop")] - - -def main(): - ws = WorldState() - is_at_goal = ws.get_state(get_goal_state()) - print(is_at_goal) - sys.exit(0) diff --git a/delib_ws_p1/delib_ws_p1/run.py b/delib_ws_p1/delib_ws_p1/run.py deleted file mode 100644 index f964dad..0000000 --- a/delib_ws_p1/delib_ws_p1/run.py +++ /dev/null @@ -1,70 +0,0 @@ -#!/usr/bin/env python3 - -""" -__Goal__: -Banana on table_sink. - -__Initial State__: -Banana on table_source. - -__Available Actions__: - -- Pick object -- Place object -- Move robot - -__Available Conditions__: - -- Robot location - dining_room, banana_farm -- Object location - table_sink, table_source -- Object in hand - true, false - -""" -import os -import rclpy -import threading - -from pyrobosim.core import WorldYamlLoader -from pyrobosim.gui import start_gui -from pyrobosim_ros.ros_interface import WorldROSWrapper -from ament_index_python.packages import get_package_share_directory - - -data_folder = os.path.join(get_package_share_directory("delib_ws_p1"), "data") - - -def create_world_from_yaml(): - world_file = os.path.join( - get_package_share_directory("delib_ws_p1"), "data", "world.yaml" - ) - return WorldYamlLoader().from_yaml(world_file) - - -def create_ros_node(): - """Initializes ROS node""" - rclpy.init() - node = WorldROSWrapper(state_pub_rate=0.1, dynamics_rate=0.01) - - # Set the world - world = create_world_from_yaml() - node.set_world(world) - - return node - - -def main(): - node = create_ros_node() - - # Start ROS node in separate thread - ros_thread = threading.Thread(target=lambda: node.start(wait_for_gui=True)) - ros_thread.start() - - # Start GUI in main thread - start_gui(node.world) - - -if __name__ == "__main__": - main() diff --git a/delib_ws_p1/package.xml b/delib_ws_p1/package.xml deleted file mode 100644 index a14c943..0000000 --- a/delib_ws_p1/package.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - - delib_ws_p1 - 1.0.0 - Problem 1 of the ROSCon24 Deliberation Workshop - Christian Henkel - Apache-2.0 - - pyrobosim_ros - problem_interface - - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - - - ament_python - - diff --git a/delib_ws_p1/setup.cfg b/delib_ws_p1/setup.cfg deleted file mode 100644 index a989c43..0000000 --- a/delib_ws_p1/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/delib_ws_p1 -[install] -install_scripts=$base/lib/delib_ws_p1 diff --git a/delib_ws_p1/setup.py b/delib_ws_p1/setup.py deleted file mode 100644 index a87f302..0000000 --- a/delib_ws_p1/setup.py +++ /dev/null @@ -1,29 +0,0 @@ -from glob import glob - -from setuptools import find_packages, setup - -package_name = "delib_ws_p1" - -setup( - name=package_name, - version="0.0.0", - packages=find_packages(exclude=["test"]), - data_files=[ - ("share/ament_index/resource_index/packages", ["resource/" + package_name]), - ("share/" + package_name, ["package.xml"]), - ("share/" + package_name + "/data", glob("data/*.*")), - ], - install_requires=["setuptools"], - zip_safe=True, - maintainer="Christian Henkel", - maintainer_email="christian.henkel2@de.bosch.com", - description="TODO: Package description", - license="Apache-2.0", - tests_require=["pytest"], - entry_points={ - "console_scripts": [ - "run = delib_ws_p1.run:main", - "is_at_goal = delib_ws_p1.is_at_goal:main", - ], - }, -) diff --git a/delib_ws_p1_py/delib_ws_p1_py/run.py b/delib_ws_p1_py/delib_ws_p1_py/run.py deleted file mode 100644 index faf21b6..0000000 --- a/delib_ws_p1_py/delib_ws_p1_py/run.py +++ /dev/null @@ -1,26 +0,0 @@ -import rclpy - -from problem_interface.perform_action import PerformAction, ACTIONS -from problem_interface.world_state import WorldState -from delib_ws_p1.is_at_goal import get_goal_state - -import logging - -logging.basicConfig(level=logging.INFO) -logger = logging.getLogger("delib_ws_p1_py") - - -def main(): - ws = WorldState() - pa = PerformAction() - assert not ws.get_state( - get_goal_state() - ), "Robot must not be at goal at the beginning" - pa.do_action(ACTIONS.NAVIGATE, "table_source") - pa.do_action(ACTIONS.PICK, "banana0") - pa.do_action(ACTIONS.NAVIGATE, "table_sink") - pa.do_action(ACTIONS.PLACE, "banana0") - assert ws.get_state(get_goal_state()), "Robot must be at goal at the end" - logger.info("Goal reached") - - rclpy.shutdown() diff --git a/delib_ws_p1_py/launch/run.launch.py b/delib_ws_p1_py/launch/run.launch.py deleted file mode 100644 index 1684512..0000000 --- a/delib_ws_p1_py/launch/run.launch.py +++ /dev/null @@ -1,15 +0,0 @@ -from launch import LaunchDescription -from launch_ros.actions import Node - -from ament_index_python.packages import get_package_share_directory - -problem_pkg = get_package_share_directory("delib_ws_p1") - - -def generate_launch_description(): - # Problem node - problem_node = Node(package="delib_ws_p1", executable="run", name="run") - # Solution node - solution_node = Node(package="delib_ws_p1_py", executable="run", name="run") - - return LaunchDescription([problem_node, solution_node]) diff --git a/delib_ws_p1_py/package.xml b/delib_ws_p1_py/package.xml deleted file mode 100644 index d63e26b..0000000 --- a/delib_ws_p1_py/package.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - - delib_ws_p1_py - 1.0.0 - Solution for problem 1 of the ROSCon24 Deliberation Workshop - Christian Henkel - Apache-2.0 - - rclpy - problem_interface - - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - - - ament_python - - diff --git a/delib_ws_p1_py/setup.cfg b/delib_ws_p1_py/setup.cfg deleted file mode 100644 index b46c86d..0000000 --- a/delib_ws_p1_py/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/delib_ws_p1_py -[install] -install_scripts=$base/lib/delib_ws_p1_py diff --git a/delib_ws_p2/data/location_data.yaml b/delib_ws_p2/data/location_data.yaml deleted file mode 100644 index 1282330..0000000 --- a/delib_ws_p2/data/location_data.yaml +++ /dev/null @@ -1,17 +0,0 @@ -############################# -# Example location metadata # -############################# - -table: - footprint: - type: box - dims: [0.6, 0.6] - height: 0.5 - nav_poses: - - [0, -0.4, 1.57] - locations: - - name: "tabletop" - footprint: - type: parent - padding: 0.1 - color: [0.2, 0.2, 0.2] diff --git a/delib_ws_p2/data/object_data.yaml b/delib_ws_p2/data/object_data.yaml deleted file mode 100644 index 2396f0f..0000000 --- a/delib_ws_p2/data/object_data.yaml +++ /dev/null @@ -1,9 +0,0 @@ -########################### -# Example object metadata # -########################### - -banana: - footprint: - type: box - dims: [0.05, 0.2] - color: [0.7, 0.7, 0] diff --git a/delib_ws_p2/delib_ws_p2/is_at_goal.py b/delib_ws_p2/delib_ws_p2/is_at_goal.py deleted file mode 100644 index 2f180fb..0000000 --- a/delib_ws_p2/delib_ws_p2/is_at_goal.py +++ /dev/null @@ -1,14 +0,0 @@ -from problem_interface.world_state import WorldState - -import sys - - -def get_goal_state(): - return [("objects.banana0.parent", "table_sink_tabletop")] - - -def main(): - ws = WorldState() - is_at_goal = ws.get_state(get_goal_state()) - print(is_at_goal) - sys.exit(0) diff --git a/delib_ws_p2/package.xml b/delib_ws_p2/package.xml deleted file mode 100644 index dc9ddb6..0000000 --- a/delib_ws_p2/package.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - - delib_ws_p2 - 1.0.0 - Problem 2 of the ROSCon24 Deliberation Workshop - Christian Henkel - Apache-2.0 - - pyrobosim_ros - problem_interface - - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - - - ament_python - - diff --git a/delib_ws_p2/setup.cfg b/delib_ws_p2/setup.cfg deleted file mode 100644 index ccedfbd..0000000 --- a/delib_ws_p2/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/delib_ws_p2 -[install] -install_scripts=$base/lib/delib_ws_p2 diff --git a/delib_ws_p2/setup.py b/delib_ws_p2/setup.py deleted file mode 100644 index 4932c5e..0000000 --- a/delib_ws_p2/setup.py +++ /dev/null @@ -1,29 +0,0 @@ -from glob import glob - -from setuptools import find_packages, setup - -package_name = "delib_ws_p2" - -setup( - name=package_name, - version="0.0.0", - packages=find_packages(exclude=["test"]), - data_files=[ - ("share/ament_index/resource_index/packages", ["resource/" + package_name]), - ("share/" + package_name, ["package.xml"]), - ("share/" + package_name + "/data", glob("data/*.*")), - ], - install_requires=["setuptools"], - zip_safe=True, - maintainer="Christian Henkel", - maintainer_email="christian.henkel2@de.bosch.com", - description="TODO: Package description", - license="Apache-2.0", - tests_require=["pytest"], - entry_points={ - "console_scripts": [ - "run = delib_ws_p2.run:main", - "is_at_goal = delib_ws_p2.is_at_goal:main", - ], - }, -) diff --git a/delib_ws_p2_py/delib_ws_p2_py/__init__.py b/delib_ws_p2_py/delib_ws_p2_py/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/delib_ws_p2_py/delib_ws_p2_py/run.py b/delib_ws_p2_py/delib_ws_p2_py/run.py deleted file mode 100644 index 8e0b43a..0000000 --- a/delib_ws_p2_py/delib_ws_p2_py/run.py +++ /dev/null @@ -1,28 +0,0 @@ -import rclpy - -from problem_interface.perform_action import PerformAction, ACTIONS -from problem_interface.world_state import WorldState -from delib_ws_p1.is_at_goal import get_goal_state - -import logging - -logging.basicConfig(level=logging.INFO) -logger = logging.getLogger("delib_ws_p2_py") - - -def main(): - ws = WorldState() - pa = PerformAction() - assert not ws.get_state( - get_goal_state() - ), "Robot must not be at goal at the beginning" - pa.do_action(ACTIONS.NAVIGATE, "hall_banana_farm_dining_room") - pa.do_action(ACTIONS.OPEN, "hall_banana_farm_dining_room") - pa.do_action(ACTIONS.NAVIGATE, "table_source") - pa.do_action(ACTIONS.PICK, "banana0") - pa.do_action(ACTIONS.NAVIGATE, "table_sink") - pa.do_action(ACTIONS.PLACE, "banana0") - assert ws.get_state(get_goal_state()), "Robot must be at goal at the end" - logger.info("Goal reached") - - rclpy.shutdown() diff --git a/delib_ws_p2_py/launch/run.launch.py b/delib_ws_p2_py/launch/run.launch.py deleted file mode 100644 index 01ba174..0000000 --- a/delib_ws_p2_py/launch/run.launch.py +++ /dev/null @@ -1,11 +0,0 @@ -from launch import LaunchDescription -from launch_ros.actions import Node - - -def generate_launch_description(): - # Problem node - problem_node = Node(package="delib_ws_p2", executable="run", name="run") - # Solution node - solution_node = Node(package="delib_ws_p2_py", executable="run", name="run") - - return LaunchDescription([problem_node, solution_node]) diff --git a/delib_ws_p2_py/resource/delib_ws_p2_py b/delib_ws_p2_py/resource/delib_ws_p2_py deleted file mode 100644 index e69de29..0000000 diff --git a/delib_ws_p2_py/setup.cfg b/delib_ws_p2_py/setup.cfg deleted file mode 100644 index ef8d283..0000000 --- a/delib_ws_p2_py/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/delib_ws_p2_py -[install] -install_scripts=$base/lib/delib_ws_p2_py diff --git a/delib_ws_p2_py/setup.py b/delib_ws_p2_py/setup.py deleted file mode 100644 index 77fb5a1..0000000 --- a/delib_ws_p2_py/setup.py +++ /dev/null @@ -1,27 +0,0 @@ -from setuptools import find_packages, setup - -import os -from glob import glob - -package_name = "delib_ws_p2_py" - -setup( - name=package_name, - version="0.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("launch/*.launch.py")), - ], - install_requires=["setuptools"], - zip_safe=True, - maintainer="hec2le", - maintainer_email="christian.henkel2@de.bosch.com", - description="TODO: Package description", - license="Apache-2.0", - tests_require=["pytest"], - entry_points={ - "console_scripts": ["run = delib_ws_p2_py.run:main"], - }, -) diff --git a/delib_ws_p2_py/test/test_copyright.py b/delib_ws_p2_py/test/test_copyright.py deleted file mode 100644 index ceffe89..0000000 --- a/delib_ws_p2_py/test/test_copyright.py +++ /dev/null @@ -1,27 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_copyright.main import main -import pytest - - -# Remove the `skip` decorator once the source file(s) have a copyright header -@pytest.mark.skip( - reason="No copyright header has been placed in the generated source file." -) -@pytest.mark.copyright -@pytest.mark.linter -def test_copyright(): - rc = main(argv=[".", "test"]) - assert rc == 0, "Found errors" diff --git a/delib_ws_p2_py/test/test_flake8.py b/delib_ws_p2_py/test/test_flake8.py deleted file mode 100644 index ee79f31..0000000 --- a/delib_ws_p2_py/test/test_flake8.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_flake8.main import main_with_errors -import pytest - - -@pytest.mark.flake8 -@pytest.mark.linter -def test_flake8(): - rc, errors = main_with_errors(argv=[]) - assert rc == 0, "Found %d code style errors / warnings:\n" % len( - errors - ) + "\n".join(errors) diff --git a/delib_ws_p2_py/test/test_pep257.py b/delib_ws_p2_py/test/test_pep257.py deleted file mode 100644 index a2c3deb..0000000 --- a/delib_ws_p2_py/test/test_pep257.py +++ /dev/null @@ -1,23 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_pep257.main import main -import pytest - - -@pytest.mark.linter -@pytest.mark.pep257 -def test_pep257(): - rc = main(argv=[".", "test"]) - assert rc == 0, "Found code style errors / warnings" diff --git a/delib_ws_p3/data/location_data.yaml b/delib_ws_p3/data/location_data.yaml deleted file mode 100644 index 1282330..0000000 --- a/delib_ws_p3/data/location_data.yaml +++ /dev/null @@ -1,17 +0,0 @@ -############################# -# Example location metadata # -############################# - -table: - footprint: - type: box - dims: [0.6, 0.6] - height: 0.5 - nav_poses: - - [0, -0.4, 1.57] - locations: - - name: "tabletop" - footprint: - type: parent - padding: 0.1 - color: [0.2, 0.2, 0.2] diff --git a/delib_ws_p3/data/object_data.yaml b/delib_ws_p3/data/object_data.yaml deleted file mode 100644 index 2396f0f..0000000 --- a/delib_ws_p3/data/object_data.yaml +++ /dev/null @@ -1,9 +0,0 @@ -########################### -# Example object metadata # -########################### - -banana: - footprint: - type: box - dims: [0.05, 0.2] - color: [0.7, 0.7, 0] diff --git a/delib_ws_p3/delib_ws_p3/__init__.py b/delib_ws_p3/delib_ws_p3/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/delib_ws_p3/delib_ws_p3/is_at_goal.py b/delib_ws_p3/delib_ws_p3/is_at_goal.py deleted file mode 100644 index 2f180fb..0000000 --- a/delib_ws_p3/delib_ws_p3/is_at_goal.py +++ /dev/null @@ -1,14 +0,0 @@ -from problem_interface.world_state import WorldState - -import sys - - -def get_goal_state(): - return [("objects.banana0.parent", "table_sink_tabletop")] - - -def main(): - ws = WorldState() - is_at_goal = ws.get_state(get_goal_state()) - print(is_at_goal) - sys.exit(0) diff --git a/delib_ws_p3/delib_ws_p3/run.py b/delib_ws_p3/delib_ws_p3/run.py deleted file mode 100644 index 21fc7ac..0000000 --- a/delib_ws_p3/delib_ws_p3/run.py +++ /dev/null @@ -1,67 +0,0 @@ -#!/usr/bin/env python3 - -""" -__Goal__: -Banana on table_sink. - -__Initial State__: -Banana on table_source. - -__Available Actions__: - -- Pick object -- Place object -- Move robot - -__Available Conditions__: - -- Robot location - dining_room, banana_farm -- Object location - table_sink, table_source -- Object in hand - true, false - -""" -import os -import rclpy -import threading - -from pyrobosim.core import WorldYamlLoader -from pyrobosim.gui import start_gui -from pyrobosim_ros.ros_interface import WorldROSWrapper -from ament_index_python.packages import get_package_share_directory - - -def create_world_from_yaml(): - world_file = os.path.join( - get_package_share_directory("delib_ws_p3"), "data", "world.yaml" - ) - return WorldYamlLoader().from_yaml(world_file) - - -def create_ros_node(): - """Initializes ROS node""" - rclpy.init() - node = WorldROSWrapper(state_pub_rate=0.1, dynamics_rate=0.01) - - # Set the world - world = create_world_from_yaml() - node.set_world(world) - - return node - - -def main(): - node = create_ros_node() - - # Start ROS node in separate thread - ros_thread = threading.Thread(target=lambda: node.start(wait_for_gui=True)) - ros_thread.start() - - # Start GUI in main thread - start_gui(node.world) - - -if __name__ == "__main__": - main() diff --git a/delib_ws_p3/resource/delib_ws_p3 b/delib_ws_p3/resource/delib_ws_p3 deleted file mode 100644 index e69de29..0000000 diff --git a/delib_ws_p3/setup.cfg b/delib_ws_p3/setup.cfg deleted file mode 100644 index 1b4625c..0000000 --- a/delib_ws_p3/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/delib_ws_p3 -[install] -install_scripts=$base/lib/delib_ws_p3 diff --git a/delib_ws_p3/test/test_copyright.py b/delib_ws_p3/test/test_copyright.py deleted file mode 100644 index ceffe89..0000000 --- a/delib_ws_p3/test/test_copyright.py +++ /dev/null @@ -1,27 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_copyright.main import main -import pytest - - -# Remove the `skip` decorator once the source file(s) have a copyright header -@pytest.mark.skip( - reason="No copyright header has been placed in the generated source file." -) -@pytest.mark.copyright -@pytest.mark.linter -def test_copyright(): - rc = main(argv=[".", "test"]) - assert rc == 0, "Found errors" diff --git a/delib_ws_p3/test/test_flake8.py b/delib_ws_p3/test/test_flake8.py deleted file mode 100644 index ee79f31..0000000 --- a/delib_ws_p3/test/test_flake8.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_flake8.main import main_with_errors -import pytest - - -@pytest.mark.flake8 -@pytest.mark.linter -def test_flake8(): - rc, errors = main_with_errors(argv=[]) - assert rc == 0, "Found %d code style errors / warnings:\n" % len( - errors - ) + "\n".join(errors) diff --git a/delib_ws_p3/test/test_pep257.py b/delib_ws_p3/test/test_pep257.py deleted file mode 100644 index a2c3deb..0000000 --- a/delib_ws_p3/test/test_pep257.py +++ /dev/null @@ -1,23 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_pep257.main import main -import pytest - - -@pytest.mark.linter -@pytest.mark.pep257 -def test_pep257(): - rc = main(argv=[".", "test"]) - assert rc == 0, "Found code style errors / warnings" diff --git a/delib_ws_p4/data/location_data.yaml b/delib_ws_p4/data/location_data.yaml deleted file mode 100644 index 1282330..0000000 --- a/delib_ws_p4/data/location_data.yaml +++ /dev/null @@ -1,17 +0,0 @@ -############################# -# Example location metadata # -############################# - -table: - footprint: - type: box - dims: [0.6, 0.6] - height: 0.5 - nav_poses: - - [0, -0.4, 1.57] - locations: - - name: "tabletop" - footprint: - type: parent - padding: 0.1 - color: [0.2, 0.2, 0.2] diff --git a/delib_ws_p4/data/object_data.yaml b/delib_ws_p4/data/object_data.yaml deleted file mode 100644 index 2396f0f..0000000 --- a/delib_ws_p4/data/object_data.yaml +++ /dev/null @@ -1,9 +0,0 @@ -########################### -# Example object metadata # -########################### - -banana: - footprint: - type: box - dims: [0.05, 0.2] - color: [0.7, 0.7, 0] diff --git a/delib_ws_p4/delib_ws_p4/__init__.py b/delib_ws_p4/delib_ws_p4/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/delib_ws_p4/delib_ws_p4/is_at_goal.py b/delib_ws_p4/delib_ws_p4/is_at_goal.py deleted file mode 100644 index 2f180fb..0000000 --- a/delib_ws_p4/delib_ws_p4/is_at_goal.py +++ /dev/null @@ -1,14 +0,0 @@ -from problem_interface.world_state import WorldState - -import sys - - -def get_goal_state(): - return [("objects.banana0.parent", "table_sink_tabletop")] - - -def main(): - ws = WorldState() - is_at_goal = ws.get_state(get_goal_state()) - print(is_at_goal) - sys.exit(0) diff --git a/delib_ws_p4/delib_ws_p4/run.py b/delib_ws_p4/delib_ws_p4/run.py deleted file mode 100644 index 4adc96b..0000000 --- a/delib_ws_p4/delib_ws_p4/run.py +++ /dev/null @@ -1,68 +0,0 @@ -#!/usr/bin/env python3 - -""" -__Goal__: -Banana on table_sink. - -__Initial State__: -Banana on table_source. - -__Available Actions__: - -- Pick object -- Place object -- Move robot - -__Available Conditions__: - -- Robot location - dining_room, banana_farm -- Object location - table_sink, table_source -- Object in hand - true, false - -""" -import os -import rclpy -import threading -import numpy as np - -from pyrobosim.core import WorldYamlLoader -from pyrobosim.gui import start_gui -from pyrobosim_ros.ros_interface import WorldROSWrapper -from ament_index_python.packages import get_package_share_directory - - -def create_world_from_yaml(): - world_file = os.path.join( - get_package_share_directory("delib_ws_p4"), "data", "world.yaml" - ) - return WorldYamlLoader().from_yaml(world_file) - - -def create_ros_node(): - """Initializes ROS node""" - rclpy.init() - node = WorldROSWrapper(state_pub_rate=0.1, dynamics_rate=0.01) - - # Set the world - world = create_world_from_yaml() - node.set_world(world) - - return node - - -def main(): - node = create_ros_node() - - # Start ROS node in separate thread - ros_thread = threading.Thread(target=lambda: node.start(wait_for_gui=True)) - ros_thread.start() - - # Start GUI in main thread - start_gui(node.world) - - -if __name__ == "__main__": - main() diff --git a/delib_ws_p4/package.xml b/delib_ws_p4/package.xml deleted file mode 100644 index a9e3d5c..0000000 --- a/delib_ws_p4/package.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - - delib_ws_p4 - 1.0.0 - Problem 2 of the ROSCon24 Deliberation Workshop - Christian Henkel - Apache-2.0 - - pyrobosim_ros - problem_interface - - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - - - ament_python - - diff --git a/delib_ws_p4/resource/delib_ws_p4 b/delib_ws_p4/resource/delib_ws_p4 deleted file mode 100644 index e69de29..0000000 diff --git a/delib_ws_p4/setup.cfg b/delib_ws_p4/setup.cfg deleted file mode 100644 index 5cd2174..0000000 --- a/delib_ws_p4/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/delib_ws_p4 -[install] -install_scripts=$base/lib/delib_ws_p4 diff --git a/delib_ws_p4/setup.py b/delib_ws_p4/setup.py deleted file mode 100644 index 0e98a5f..0000000 --- a/delib_ws_p4/setup.py +++ /dev/null @@ -1,29 +0,0 @@ -from glob import glob - -from setuptools import find_packages, setup - -package_name = "delib_ws_p4" - -setup( - name=package_name, - version="0.0.0", - packages=find_packages(exclude=["test"]), - data_files=[ - ("share/ament_index/resource_index/packages", ["resource/" + package_name]), - ("share/" + package_name, ["package.xml"]), - ("share/" + package_name + "/data", glob("data/*.*")), - ], - install_requires=["setuptools"], - zip_safe=True, - maintainer="Christian Henkel", - maintainer_email="christian.henkel2@de.bosch.com", - description="TODO: Package description", - license="Apache-2.0", - tests_require=["pytest"], - entry_points={ - "console_scripts": [ - "run = delib_ws_p4.run:main", - "is_at_goal = delib_ws_p4.is_at_goal:main", - ], - }, -) diff --git a/delib_ws_p4/test/test_copyright.py b/delib_ws_p4/test/test_copyright.py deleted file mode 100644 index ceffe89..0000000 --- a/delib_ws_p4/test/test_copyright.py +++ /dev/null @@ -1,27 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_copyright.main import main -import pytest - - -# Remove the `skip` decorator once the source file(s) have a copyright header -@pytest.mark.skip( - reason="No copyright header has been placed in the generated source file." -) -@pytest.mark.copyright -@pytest.mark.linter -def test_copyright(): - rc = main(argv=[".", "test"]) - assert rc == 0, "Found errors" diff --git a/delib_ws_p4/test/test_flake8.py b/delib_ws_p4/test/test_flake8.py deleted file mode 100644 index ee79f31..0000000 --- a/delib_ws_p4/test/test_flake8.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_flake8.main import main_with_errors -import pytest - - -@pytest.mark.flake8 -@pytest.mark.linter -def test_flake8(): - rc, errors = main_with_errors(argv=[]) - assert rc == 0, "Found %d code style errors / warnings:\n" % len( - errors - ) + "\n".join(errors) diff --git a/delib_ws_p4/test/test_pep257.py b/delib_ws_p4/test/test_pep257.py deleted file mode 100644 index a2c3deb..0000000 --- a/delib_ws_p4/test/test_pep257.py +++ /dev/null @@ -1,23 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_pep257.main import main -import pytest - - -@pytest.mark.linter -@pytest.mark.pep257 -def test_pep257(): - rc = main(argv=[".", "test"]) - assert rc == 0, "Found code style errors / warnings" diff --git a/delib_ws_p1_py/LICENSE b/delib_ws_problem_interface/LICENSE similarity index 100% rename from delib_ws_p1_py/LICENSE rename to delib_ws_problem_interface/LICENSE diff --git a/delib_ws_p1/delib_ws_p1/__init__.py b/delib_ws_problem_interface/delib_ws_problem_interface/__init__.py similarity index 100% rename from delib_ws_p1/delib_ws_p1/__init__.py rename to delib_ws_problem_interface/delib_ws_problem_interface/__init__.py diff --git a/problem_interface/problem_interface/perform_action.py b/delib_ws_problem_interface/delib_ws_problem_interface/perform_action.py similarity index 100% rename from problem_interface/problem_interface/perform_action.py rename to delib_ws_problem_interface/delib_ws_problem_interface/perform_action.py diff --git a/problem_interface/problem_interface/world_state.py b/delib_ws_problem_interface/delib_ws_problem_interface/world_state.py similarity index 88% rename from problem_interface/problem_interface/world_state.py rename to delib_ws_problem_interface/delib_ws_problem_interface/world_state.py index bb32949..cf7bd9d 100644 --- a/problem_interface/problem_interface/world_state.py +++ b/delib_ws_problem_interface/delib_ws_problem_interface/world_state.py @@ -18,6 +18,7 @@ def __init__(self): if not rclpy.ok(): rclpy.init() self.node = rclpy.create_node("world_state") + self.node.declare_parameter("problem_number", 1) self._client = self.node.create_client( RequestWorldState, "/request_world_state" ) @@ -27,9 +28,15 @@ def __init__(self): self.wait: bool = False self.expected_state: List[Tuple[str, str]] = [] - def get_state(self, expected_state) -> bool: + def get_problem_number(self) -> int: """ - Call the service to get the current world state. + Returns the problem number configured via ROS parameter. + """ + return self.node.get_parameter("problem_number").value + + def check_state(self, expected_state) -> bool: + """ + Call the service to get the current world state and check it against an expected state. :param expected_state: The expected state of the world. List of tuples of the form (key, value). diff --git a/problem_interface/package.xml b/delib_ws_problem_interface/package.xml similarity index 79% rename from problem_interface/package.xml rename to delib_ws_problem_interface/package.xml index 70d2a01..205941a 100644 --- a/problem_interface/package.xml +++ b/delib_ws_problem_interface/package.xml @@ -1,9 +1,9 @@ - problem_interface - 0.0.0 - TODO: Package description + delib_ws_problem_interface + 1.0.0 + Problem interface for ROS 2 Deliberation Workshop hec2le Apache-2.0 diff --git a/delib_ws_p1/resource/delib_ws_p1 b/delib_ws_problem_interface/resource/delib_ws_problem_interface similarity index 100% rename from delib_ws_p1/resource/delib_ws_p1 rename to delib_ws_problem_interface/resource/delib_ws_problem_interface diff --git a/delib_ws_problem_interface/setup.cfg b/delib_ws_problem_interface/setup.cfg new file mode 100644 index 0000000..c5cc0a7 --- /dev/null +++ b/delib_ws_problem_interface/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/delib_ws_problem_interface +[install] +install_scripts=$base/lib/delib_ws_problem_interface diff --git a/problem_interface/setup.py b/delib_ws_problem_interface/setup.py similarity index 79% rename from problem_interface/setup.py rename to delib_ws_problem_interface/setup.py index b302871..02afd04 100644 --- a/problem_interface/setup.py +++ b/delib_ws_problem_interface/setup.py @@ -1,10 +1,10 @@ from setuptools import find_packages, setup -package_name = "problem_interface" +package_name = "delib_ws_problem_interface" setup( name=package_name, - version="0.0.0", + version="1.0.0", packages=find_packages(exclude=["test"]), data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), @@ -14,7 +14,7 @@ zip_safe=True, maintainer="hec2le", maintainer_email="christian.henkel2@de.bosch.com", - description="TODO: Package description", + description="Problem interface for ROS 2 Deliberation Workshop", license="Apache-2.0", tests_require=["pytest"], entry_points={ diff --git a/delib_ws_p1/test/test_copyright.py b/delib_ws_problem_interface/test/test_copyright.py similarity index 100% rename from delib_ws_p1/test/test_copyright.py rename to delib_ws_problem_interface/test/test_copyright.py diff --git a/delib_ws_p1/test/test_flake8.py b/delib_ws_problem_interface/test/test_flake8.py similarity index 100% rename from delib_ws_p1/test/test_flake8.py rename to delib_ws_problem_interface/test/test_flake8.py diff --git a/delib_ws_p1/test/test_pep257.py b/delib_ws_problem_interface/test/test_pep257.py similarity index 100% rename from delib_ws_p1/test/test_pep257.py rename to delib_ws_problem_interface/test/test_pep257.py diff --git a/delib_ws_p2_py/LICENSE b/delib_ws_python/LICENSE similarity index 100% rename from delib_ws_p2_py/LICENSE rename to delib_ws_python/LICENSE diff --git a/delib_ws_p1_py/delib_ws_p1_py/__init__.py b/delib_ws_python/delib_ws_python/__init__.py similarity index 100% rename from delib_ws_p1_py/delib_ws_p1_py/__init__.py rename to delib_ws_python/delib_ws_python/__init__.py diff --git a/delib_ws_python/delib_ws_python/run.py b/delib_ws_python/delib_ws_python/run.py new file mode 100644 index 0000000..41f80a8 --- /dev/null +++ b/delib_ws_python/delib_ws_python/run.py @@ -0,0 +1,48 @@ +import rclpy + +from delib_ws_worlds.is_at_goal import get_goal_state +from delib_ws_python import solutions +from delib_ws_problem_interface.perform_action import PerformAction +from delib_ws_problem_interface.world_state import WorldState + +import logging + +logging.basicConfig(level=logging.INFO) +logger = logging.getLogger("delib_ws_python") + + +def get_solution(problem_number): + if problem_number == 1: + return solutions.problem1() + elif problem_number == 2: + return solutions.problem2() + elif problem_number == 3: + return solutions.problem3() + elif problem_number == 4: + return solutions.problem4() + else: + raise ValueError(f"No solution implemented for problem {problem_number}") + + +def execute_solution(problem_number): + solution = get_solution(problem_number) + + pa = PerformAction() + for action, argument in solution: + pa.do_action(action, argument) + + +def main(): + ws = WorldState() + expected_state = get_goal_state(ws.get_problem_number()) + + assert not ws.check_state( + expected_state + ), "Robot must not be at goal at the beginning" + + execute_solution(ws.get_problem_number()) + + assert ws.check_state(expected_state), "Robot must be at goal at the end" + logger.info("Goal reached") + + rclpy.shutdown() diff --git a/delib_ws_python/delib_ws_python/solutions.py b/delib_ws_python/delib_ws_python/solutions.py new file mode 100644 index 0000000..b420e81 --- /dev/null +++ b/delib_ws_python/delib_ws_python/solutions.py @@ -0,0 +1,37 @@ +""" +Solution implementations in Python. +""" + +from delib_ws_problem_interface.perform_action import ACTIONS + + +def problem1(): + """Gets the Problem 1 solution""" + return ( + (ACTIONS.NAVIGATE, "table_source"), + (ACTIONS.PICK, "banana0"), + (ACTIONS.NAVIGATE, "table_sink"), + (ACTIONS.PLACE, "banana0"), + ) + + +def problem2(): + """Gets the Problem 2 solution""" + return ( + (ACTIONS.NAVIGATE, "hall_banana_farm_dining_room"), + (ACTIONS.OPEN, "hall_banana_farm_dining_room"), + (ACTIONS.NAVIGATE, "table_source"), + (ACTIONS.PICK, "banana0"), + (ACTIONS.NAVIGATE, "table_sink"), + (ACTIONS.PLACE, "banana0"), + ) + + +def problem3(): + """Gets the Problem 3 solution""" + return () # TODO: Implement solution + + +def problem4(): + """Gets the Problem 4 solution""" + return () # TODO: Implement solution diff --git a/delib_ws_python/launch/run.launch.py b/delib_ws_python/launch/run.launch.py new file mode 100644 index 0000000..1ed887f --- /dev/null +++ b/delib_ws_python/launch/run.launch.py @@ -0,0 +1,25 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + # Problem number argument + problem_number_arg = DeclareLaunchArgument("problem_number", default_value="1") + # Problem node + problem_node = Node( + package="delib_ws_worlds", + executable="run", + name="run_world", + parameters=[{"problem_number": LaunchConfiguration("problem_number")}], + ) + # Solution node + solution_node = Node( + package="delib_ws_python", + executable="run", + name="run_solution", + parameters=[{"problem_number": LaunchConfiguration("problem_number")}], + ) + + return LaunchDescription([problem_number_arg, problem_node, solution_node]) diff --git a/delib_ws_p2_py/package.xml b/delib_ws_python/package.xml similarity index 78% rename from delib_ws_p2_py/package.xml rename to delib_ws_python/package.xml index 48eff54..a7fa5d4 100644 --- a/delib_ws_p2_py/package.xml +++ b/delib_ws_python/package.xml @@ -1,14 +1,14 @@ - delib_ws_p2_py + delib_ws_python 1.0.0 - Solution for problem 1 of the ROSCon24 Deliberation Workshop + Simple Python solutions to ROS 2 Deliberation Workshop Christian Henkel Apache-2.0 rclpy - problem_interface + delib_ws_problem_interface ament_copyright ament_flake8 diff --git a/delib_ws_p1_py/resource/delib_ws_p1_py b/delib_ws_python/resource/delib_ws_python similarity index 100% rename from delib_ws_p1_py/resource/delib_ws_p1_py rename to delib_ws_python/resource/delib_ws_python diff --git a/delib_ws_python/setup.cfg b/delib_ws_python/setup.cfg new file mode 100644 index 0000000..ef50b02 --- /dev/null +++ b/delib_ws_python/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/delib_ws_python +[install] +install_scripts=$base/lib/delib_ws_python diff --git a/delib_ws_p1_py/setup.py b/delib_ws_python/setup.py similarity index 79% rename from delib_ws_p1_py/setup.py rename to delib_ws_python/setup.py index 48fe4f3..c3ca31d 100644 --- a/delib_ws_p1_py/setup.py +++ b/delib_ws_python/setup.py @@ -3,7 +3,7 @@ import os from glob import glob -package_name = "delib_ws_p1_py" +package_name = "delib_ws_python" setup( name=package_name, @@ -18,10 +18,10 @@ zip_safe=True, maintainer="hec2le", maintainer_email="christian.henkel2@de.bosch.com", - description="TODO: Package description", + description="Simple Python solutions to ROS 2 Deliberation Workshop", license="Apache-2.0", tests_require=["pytest"], entry_points={ - "console_scripts": ["run = delib_ws_p1_py.run:main"], + "console_scripts": ["run = delib_ws_python.run:main"], }, ) diff --git a/delib_ws_p1_py/test/test_copyright.py b/delib_ws_python/test/test_copyright.py similarity index 100% rename from delib_ws_p1_py/test/test_copyright.py rename to delib_ws_python/test/test_copyright.py diff --git a/delib_ws_p1_py/test/test_flake8.py b/delib_ws_python/test/test_flake8.py similarity index 100% rename from delib_ws_p1_py/test/test_flake8.py rename to delib_ws_python/test/test_flake8.py diff --git a/delib_ws_p1_py/test/test_pep257.py b/delib_ws_python/test/test_pep257.py similarity index 100% rename from delib_ws_p1_py/test/test_pep257.py rename to delib_ws_python/test/test_pep257.py diff --git a/delib_ws_p2/delib_ws_p2/__init__.py b/delib_ws_worlds/delib_ws_worlds/__init__.py similarity index 100% rename from delib_ws_p2/delib_ws_p2/__init__.py rename to delib_ws_worlds/delib_ws_worlds/__init__.py diff --git a/delib_ws_worlds/delib_ws_worlds/is_at_goal.py b/delib_ws_worlds/delib_ws_worlds/is_at_goal.py new file mode 100644 index 0000000..af4d53a --- /dev/null +++ b/delib_ws_worlds/delib_ws_worlds/is_at_goal.py @@ -0,0 +1,23 @@ +""" +Checks if a goal has been reached and validates against the expected goal for that problem. +""" + +import sys + +from delib_ws_problem_interface.world_state import WorldState + + +def get_goal_state(problem_number): + """Returns the goal state based on the problem number specified.""" + if problem_number in (1, 2, 3, 4): + return (("objects.banana0.parent", "table_sink_tabletop"),) + else: + raise ValueError(f"No goal state for problem number: {problem_number}") + + +def main(): + ws = WorldState() + expected_state = get_goal_state(ws.get_problem_number()) + is_at_goal = ws.check_state(expected_state) + print(f"Is at goal: {is_at_goal}") + sys.exit(0) diff --git a/delib_ws_p2/delib_ws_p2/run.py b/delib_ws_worlds/delib_ws_worlds/run.py similarity index 60% rename from delib_ws_p2/delib_ws_p2/run.py rename to delib_ws_worlds/delib_ws_worlds/run.py index d67df90..930f499 100644 --- a/delib_ws_p2/delib_ws_p2/run.py +++ b/delib_ws_worlds/delib_ws_worlds/run.py @@ -1,28 +1,9 @@ #!/usr/bin/env python3 """ -__Goal__: -Banana on table_sink. - -__Initial State__: -Banana on table_source. - -__Available Actions__: - -- Pick object -- Place object -- Move robot - -__Available Conditions__: - -- Robot location - dining_room, banana_farm -- Object location - table_sink, table_source -- Object in hand - true, false - +Runner for ROS 2 Deliberation workshop worlds. """ + import os import rclpy import threading @@ -33,20 +14,21 @@ from ament_index_python.packages import get_package_share_directory -def create_world_from_yaml(): - world_file = os.path.join( - get_package_share_directory("delib_ws_p2"), "data", "world.yaml" - ) - return WorldYamlLoader().from_yaml(world_file) - - def create_ros_node(): """Initializes ROS node""" rclpy.init() node = WorldROSWrapper(state_pub_rate=0.1, dynamics_rate=0.01) + node.declare_parameter("problem_number", 1) - # Set the world - world = create_world_from_yaml() + # Set the world file. + problem_number = node.get_parameter("problem_number").value + node.get_logger().info(f"Starting problem number {problem_number}") + world_file = os.path.join( + get_package_share_directory("delib_ws_worlds"), + "worlds", + f"world{problem_number}.yaml", + ) + world = WorldYamlLoader().from_yaml(world_file) node.set_world(world) return node diff --git a/delib_ws_p3/package.xml b/delib_ws_worlds/package.xml similarity index 78% rename from delib_ws_p3/package.xml rename to delib_ws_worlds/package.xml index b7e212f..e7cd3f2 100644 --- a/delib_ws_p3/package.xml +++ b/delib_ws_worlds/package.xml @@ -1,14 +1,14 @@ - delib_ws_p3 + delib_ws_worlds 1.0.0 - Problem 2 of the ROSCon24 Deliberation Workshop + Example worlds for the ROS 2 Deliberation Workshop Christian Henkel Apache-2.0 pyrobosim_ros - problem_interface + delib_ws_problem_interface ament_copyright ament_flake8 diff --git a/delib_ws_p2/resource/delib_ws_p2 b/delib_ws_worlds/resource/delib_ws_worlds similarity index 100% rename from delib_ws_p2/resource/delib_ws_p2 rename to delib_ws_worlds/resource/delib_ws_worlds diff --git a/delib_ws_worlds/setup.cfg b/delib_ws_worlds/setup.cfg new file mode 100644 index 0000000..f269d41 --- /dev/null +++ b/delib_ws_worlds/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/delib_ws_worlds +[install] +install_scripts=$base/lib/delib_ws_worlds diff --git a/delib_ws_p3/setup.py b/delib_ws_worlds/setup.py similarity index 65% rename from delib_ws_p3/setup.py rename to delib_ws_worlds/setup.py index 42da613..8bdaa25 100644 --- a/delib_ws_p3/setup.py +++ b/delib_ws_worlds/setup.py @@ -2,28 +2,28 @@ from setuptools import find_packages, setup -package_name = "delib_ws_p3" +package_name = "delib_ws_worlds" setup( name=package_name, - version="0.0.0", + 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"]), - ("share/" + package_name + "/data", glob("data/*.*")), + ("share/" + package_name + "/worlds", glob("worlds/*.*")), ], install_requires=["setuptools"], zip_safe=True, maintainer="Christian Henkel", maintainer_email="christian.henkel2@de.bosch.com", - description="TODO: Package description", + description="Example worlds for the ROS 2 Deliberation Workshop", license="Apache-2.0", tests_require=["pytest"], entry_points={ "console_scripts": [ - "run = delib_ws_p3.run:main", - "is_at_goal = delib_ws_p3.is_at_goal:main", + "run = delib_ws_worlds.run:main", + "is_at_goal = delib_ws_worlds.is_at_goal:main", ], }, ) diff --git a/delib_ws_p2/test/test_copyright.py b/delib_ws_worlds/test/test_copyright.py similarity index 100% rename from delib_ws_p2/test/test_copyright.py rename to delib_ws_worlds/test/test_copyright.py diff --git a/delib_ws_p2/test/test_flake8.py b/delib_ws_worlds/test/test_flake8.py similarity index 100% rename from delib_ws_p2/test/test_flake8.py rename to delib_ws_worlds/test/test_flake8.py diff --git a/delib_ws_p2/test/test_pep257.py b/delib_ws_worlds/test/test_pep257.py similarity index 100% rename from delib_ws_p2/test/test_pep257.py rename to delib_ws_worlds/test/test_pep257.py diff --git a/delib_ws_p1/data/location_data.yaml b/delib_ws_worlds/worlds/location_data.yaml similarity index 100% rename from delib_ws_p1/data/location_data.yaml rename to delib_ws_worlds/worlds/location_data.yaml diff --git a/delib_ws_p1/data/object_data.yaml b/delib_ws_worlds/worlds/object_data.yaml similarity index 100% rename from delib_ws_p1/data/object_data.yaml rename to delib_ws_worlds/worlds/object_data.yaml diff --git a/delib_ws_p1/data/world.yaml b/delib_ws_worlds/worlds/world1.yaml similarity index 74% rename from delib_ws_p1/data/world.yaml rename to delib_ws_worlds/worlds/world1.yaml index 1d009b0..0ad2316 100644 --- a/delib_ws_p1/data/world.yaml +++ b/delib_ws_worlds/worlds/world1.yaml @@ -1,3 +1,24 @@ +# Problem 1 World +# +# __Goal__: +# - Banana on table_sink. +# +# __Initial State__: +# - Banana on table_source. +# +# __Available Actions__: +# - Pick object +# - Place object +# - Move robot +# +# __Available Conditions__: +# - Robot location +# dining_room, banana_farm +# - Object location +# table_sink, table_source +# - Object in hand +# true, false + metadata: locations: $PWD/location_data.yaml objects: $PWD/object_data.yaml diff --git a/delib_ws_p4/data/world.yaml b/delib_ws_worlds/worlds/world2.yaml similarity index 72% rename from delib_ws_p4/data/world.yaml rename to delib_ws_worlds/worlds/world2.yaml index 99a517d..288ce29 100644 --- a/delib_ws_p4/data/world.yaml +++ b/delib_ws_worlds/worlds/world2.yaml @@ -1,3 +1,27 @@ +# Problem 2 World +# +# __Goal__: +# - Banana on table_sink. +# +# __Initial State__: +# - Banana on table_source. +# - Door closed +# +# __Available Actions__: +# - Pick object +# - Place object +# - Move robot +# - Open door +# - Close door +# +# __Available Conditions__: +# - Robot location +# dining_room, banana_farm +# - Object location +# table_sink, table_source +# - Object in hand +# true, false + metadata: locations: $PWD/location_data.yaml objects: $PWD/object_data.yaml @@ -11,16 +35,6 @@ robots: path_planner: type: rrt rrt_star: true - action_execution_options: - navigate: - success_probability: 0.9 - rng_seed: 42 - battery_usage: 1 - pick: - success_probability: 0.5 - battery_usage: 1 - place: - battery_usage: 1 rooms: - name: banana_farm @@ -61,11 +75,6 @@ locations: parent: dining_room category: table pose: [1.5, 0.5] - - name: charging_station - parent: dining_room - category: charging_station - pose: [1.5, -0.5] - is_charger: true objects: - parent: table_source diff --git a/delib_ws_p3/data/world.yaml b/delib_ws_worlds/worlds/world3.yaml similarity index 76% rename from delib_ws_p3/data/world.yaml rename to delib_ws_worlds/worlds/world3.yaml index 0dc608d..df9952a 100644 --- a/delib_ws_p3/data/world.yaml +++ b/delib_ws_worlds/worlds/world3.yaml @@ -1,3 +1,27 @@ +# Problem 3 World +# +# __Goal__: +# - Banana on table_sink. +# +# __Initial State__: +# - Banana on table_source. +# - Door closed +# +# __Available Actions__: +# - Pick object +# - Place object +# - Move robot +# - Open door +# - Close door +# +# __Available Conditions__: +# - Robot location +# dining_room, banana_farm +# - Object location +# table_sink, table_source +# - Object in hand +# true, false + metadata: locations: $PWD/location_data.yaml objects: $PWD/object_data.yaml diff --git a/delib_ws_p2/data/world.yaml b/delib_ws_worlds/worlds/world4.yaml similarity index 54% rename from delib_ws_p2/data/world.yaml rename to delib_ws_worlds/worlds/world4.yaml index 8bd281f..e0c9b4a 100644 --- a/delib_ws_p2/data/world.yaml +++ b/delib_ws_worlds/worlds/world4.yaml @@ -1,3 +1,27 @@ +# Problem 4 World +# +# __Goal__: +# - Banana on table_sink. +# +# __Initial State__: +# - Banana on table_source. +# - Door closed +# +# __Available Actions__: +# - Pick object +# - Place object +# - Move robot +# - Open door +# - Close door +# +# __Available Conditions__: +# - Robot location +# dining_room, banana_farm +# - Object location +# table_sink, table_source +# - Object in hand +# true, false + metadata: locations: $PWD/location_data.yaml objects: $PWD/object_data.yaml @@ -11,6 +35,21 @@ robots: path_planner: type: rrt rrt_star: true + action_execution_options: + navigate: + success_probability: 0.9 + rng_seed: 42 + battery_usage: 1 + pick: + success_probability: 0.5 + battery_usage: 5 + place: + battery_usage: 5 + open: + success_probability: 0.75 + battery_usage: 10 + close: + battery_usage: 10 rooms: - name: banana_farm @@ -51,6 +90,11 @@ locations: parent: dining_room category: table pose: [1.5, 0.5] + - name: charging_station + parent: dining_room + category: charging_station + pose: [1.5, -0.5] + is_charger: true objects: - parent: table_source diff --git a/problem_interface/LICENSE b/problem_interface/LICENSE deleted file mode 100644 index d645695..0000000 --- a/problem_interface/LICENSE +++ /dev/null @@ -1,202 +0,0 @@ - - Apache License - Version 2.0, January 2004 - http://www.apache.org/licenses/ - - TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION - - 1. Definitions. - - "License" shall mean the terms and conditions for use, reproduction, - and distribution as defined by Sections 1 through 9 of this document. - - "Licensor" shall mean the copyright owner or entity authorized by - the copyright owner that is granting the License. - - "Legal Entity" shall mean the union of the acting entity and all - other entities that control, are controlled by, or are under common - control with that entity. For the purposes of this definition, - "control" means (i) the power, direct or indirect, to cause the - direction or management of such entity, whether by contract or - otherwise, or (ii) ownership of fifty percent (50%) or more of the - outstanding shares, or (iii) beneficial ownership of such entity. - - "You" (or "Your") shall mean an individual or Legal Entity - exercising permissions granted by this License. - - "Source" form shall mean the preferred form for making modifications, - including but not limited to software source code, documentation - source, and configuration files. - - "Object" form shall mean any form resulting from mechanical - transformation or translation of a Source form, including but - not limited to compiled object code, generated documentation, - and conversions to other media types. - - "Work" shall mean the work of authorship, whether in Source or - Object form, made available under the License, as indicated by a - copyright notice that is included in or attached to the work - (an example is provided in the Appendix below). - - "Derivative Works" shall mean any work, whether in Source or Object - form, that is based on (or derived from) the Work and for which the - editorial revisions, annotations, elaborations, or other modifications - represent, as a whole, an original work of authorship. For the purposes - of this License, Derivative Works shall not include works that remain - separable from, or merely link (or bind by name) to the interfaces of, - the Work and Derivative Works thereof. - - "Contribution" shall mean any work of authorship, including - the original version of the Work and any modifications or additions - to that Work or Derivative Works thereof, that is intentionally - submitted to Licensor for inclusion in the Work by the copyright owner - or by an individual or Legal Entity authorized to submit on behalf of - the copyright owner. For the purposes of this definition, "submitted" - means any form of electronic, verbal, or written communication sent - to the Licensor or its representatives, including but not limited to - communication on electronic mailing lists, source code control systems, - and issue tracking systems that are managed by, or on behalf of, the - Licensor for the purpose of discussing and improving the Work, but - excluding communication that is conspicuously marked or otherwise - designated in writing by the copyright owner as "Not a Contribution." - - "Contributor" shall mean Licensor and any individual or Legal Entity - on behalf of whom a Contribution has been received by Licensor and - subsequently incorporated within the Work. - - 2. Grant of Copyright License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - copyright license to reproduce, prepare Derivative Works of, - publicly display, publicly perform, sublicense, and distribute the - Work and such Derivative Works in Source or Object form. - - 3. Grant of Patent License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - (except as stated in this section) patent license to make, have made, - use, offer to sell, sell, import, and otherwise transfer the Work, - where such license applies only to those patent claims licensable - by such Contributor that are necessarily infringed by their - Contribution(s) alone or by combination of their Contribution(s) - with the Work to which such Contribution(s) was submitted. If You - institute patent litigation against any entity (including a - cross-claim or counterclaim in a lawsuit) alleging that the Work - or a Contribution incorporated within the Work constitutes direct - or contributory patent infringement, then any patent licenses - granted to You under this License for that Work shall terminate - as of the date such litigation is filed. - - 4. Redistribution. You may reproduce and distribute copies of the - Work or Derivative Works thereof in any medium, with or without - modifications, and in Source or Object form, provided that You - meet the following conditions: - - (a) You must give any other recipients of the Work or - Derivative Works a copy of this License; and - - (b) You must cause any modified files to carry prominent notices - stating that You changed the files; and - - (c) You must retain, in the Source form of any Derivative Works - that You distribute, all copyright, patent, trademark, and - attribution notices from the Source form of the Work, - excluding those notices that do not pertain to any part of - the Derivative Works; and - - (d) If the Work includes a "NOTICE" text file as part of its - distribution, then any Derivative Works that You distribute must - include a readable copy of the attribution notices contained - within such NOTICE file, excluding those notices that do not - pertain to any part of the Derivative Works, in at least one - of the following places: within a NOTICE text file distributed - as part of the Derivative Works; within the Source form or - documentation, if provided along with the Derivative Works; or, - within a display generated by the Derivative Works, if and - wherever such third-party notices normally appear. The contents - of the NOTICE file are for informational purposes only and - do not modify the License. You may add Your own attribution - notices within Derivative Works that You distribute, alongside - or as an addendum to the NOTICE text from the Work, provided - that such additional attribution notices cannot be construed - as modifying the License. - - You may add Your own copyright statement to Your modifications and - may provide additional or different license terms and conditions - for use, reproduction, or distribution of Your modifications, or - for any such Derivative Works as a whole, provided Your use, - reproduction, and distribution of the Work otherwise complies with - the conditions stated in this License. - - 5. Submission of Contributions. Unless You explicitly state otherwise, - any Contribution intentionally submitted for inclusion in the Work - by You to the Licensor shall be under the terms and conditions of - this License, without any additional terms or conditions. - Notwithstanding the above, nothing herein shall supersede or modify - the terms of any separate license agreement you may have executed - with Licensor regarding such Contributions. - - 6. Trademarks. This License does not grant permission to use the trade - names, trademarks, service marks, or product names of the Licensor, - except as required for reasonable and customary use in describing the - origin of the Work and reproducing the content of the NOTICE file. - - 7. Disclaimer of Warranty. Unless required by applicable law or - agreed to in writing, Licensor provides the Work (and each - Contributor provides its Contributions) on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or - implied, including, without limitation, any warranties or conditions - of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A - PARTICULAR PURPOSE. You are solely responsible for determining the - appropriateness of using or redistributing the Work and assume any - risks associated with Your exercise of permissions under this License. - - 8. Limitation of Liability. In no event and under no legal theory, - whether in tort (including negligence), contract, or otherwise, - unless required by applicable law (such as deliberate and grossly - negligent acts) or agreed to in writing, shall any Contributor be - liable to You for damages, including any direct, indirect, special, - incidental, or consequential damages of any character arising as a - result of this License or out of the use or inability to use the - Work (including but not limited to damages for loss of goodwill, - work stoppage, computer failure or malfunction, or any and all - other commercial damages or losses), even if such Contributor - has been advised of the possibility of such damages. - - 9. Accepting Warranty or Additional Liability. While redistributing - the Work or Derivative Works thereof, You may choose to offer, - and charge a fee for, acceptance of support, warranty, indemnity, - or other liability obligations and/or rights consistent with this - License. However, in accepting such obligations, You may act only - on Your own behalf and on Your sole responsibility, not on behalf - of any other Contributor, and only if You agree to indemnify, - defend, and hold each Contributor harmless for any liability - incurred by, or claims asserted against, such Contributor by reason - of your accepting any such warranty or additional liability. - - END OF TERMS AND CONDITIONS - - APPENDIX: How to apply the Apache License to your work. - - To apply the Apache License to your work, attach the following - boilerplate notice, with the fields enclosed by brackets "[]" - replaced with your own identifying information. (Don't include - the brackets!) The text should be enclosed in the appropriate - comment syntax for the file format. We also recommend that a - file or class name and description of purpose be included on the - same "printed page" as the copyright notice for easier - identification within third-party archives. - - Copyright [yyyy] [name of copyright owner] - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. diff --git a/problem_interface/problem_interface/__init__.py b/problem_interface/problem_interface/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/problem_interface/resource/problem_interface b/problem_interface/resource/problem_interface deleted file mode 100644 index e69de29..0000000 diff --git a/problem_interface/setup.cfg b/problem_interface/setup.cfg deleted file mode 100644 index 4d39a5b..0000000 --- a/problem_interface/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/problem_interface -[install] -install_scripts=$base/lib/problem_interface diff --git a/problem_interface/test/test_copyright.py b/problem_interface/test/test_copyright.py deleted file mode 100644 index ceffe89..0000000 --- a/problem_interface/test/test_copyright.py +++ /dev/null @@ -1,27 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_copyright.main import main -import pytest - - -# Remove the `skip` decorator once the source file(s) have a copyright header -@pytest.mark.skip( - reason="No copyright header has been placed in the generated source file." -) -@pytest.mark.copyright -@pytest.mark.linter -def test_copyright(): - rc = main(argv=[".", "test"]) - assert rc == 0, "Found errors" diff --git a/problem_interface/test/test_flake8.py b/problem_interface/test/test_flake8.py deleted file mode 100644 index ee79f31..0000000 --- a/problem_interface/test/test_flake8.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_flake8.main import main_with_errors -import pytest - - -@pytest.mark.flake8 -@pytest.mark.linter -def test_flake8(): - rc, errors = main_with_errors(argv=[]) - assert rc == 0, "Found %d code style errors / warnings:\n" % len( - errors - ) + "\n".join(errors) diff --git a/problem_interface/test/test_pep257.py b/problem_interface/test/test_pep257.py deleted file mode 100644 index a2c3deb..0000000 --- a/problem_interface/test/test_pep257.py +++ /dev/null @@ -1,23 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_pep257.main import main -import pytest - - -@pytest.mark.linter -@pytest.mark.pep257 -def test_pep257(): - rc = main(argv=[".", "test"]) - assert rc == 0, "Found code style errors / warnings" From 00e6bdeb28800bc31e9fdd1178d99dda04c2b6fa Mon Sep 17 00:00:00 2001 From: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Date: Mon, 2 Sep 2024 12:20:12 -0400 Subject: [PATCH 21/51] Create new home robot worlds (#19) --- README.md | 93 ++----- delib_ws_python/delib_ws_python/solutions.py | 58 +++- delib_ws_worlds/delib_ws_worlds/is_at_goal.py | 19 +- delib_ws_worlds/worlds/location_data.yaml | 99 ++++++- delib_ws_worlds/worlds/object_data.yaml | 36 ++- delib_ws_worlds/worlds/world1.yaml | 231 ++++++++++++---- delib_ws_worlds/worlds/world2.yaml | 237 ++++++++++++---- delib_ws_worlds/worlds/world3.yaml | 247 +++++++++++++---- delib_ws_worlds/worlds/world4.yaml | 253 ++++++++++++++---- dependencies/pyrobosim | 2 +- 10 files changed, 979 insertions(+), 296 deletions(-) diff --git a/README.md b/README.md index 1e841b1..6192cc6 100644 --- a/README.md +++ b/README.md @@ -39,6 +39,8 @@ you can run this command: sudo rm -rf .colcon/build .colcon/install .colcon/log ``` +--- + ## Problem Descriptions ### Problem 1 @@ -48,10 +50,10 @@ ros2 run delib_ws_worlds run --ros-args -p problem_number:=1 ``` __Goal__: -Banana on table 2. +Snacks on the dining room table. __Initial State__: -Banana on table 1. +Snacks in the kitchen pantry. __Available Actions__: @@ -59,15 +61,6 @@ __Available Actions__: - Place object - Move robot -__Available Conditions__: - -- Robot location - table 1, table 2 -- Object location - table 1, table 2 -- Object in hand - true, false - ### Problem 2 ```bash @@ -75,10 +68,12 @@ ros2 run delib_ws_worlds run --ros-args -p problem_number:=2 ``` __Goal__: -Banana on table 2. +Waste should be in the dumpster. +Dumpster should be closed. __Initial State__: -Banana on table 1. +Waste is on the office desk and in the office bin. +Hallways into the trash room are closed. __Available Actions__: @@ -89,17 +84,6 @@ __Available Actions__: - Open door - Close door -__Available Conditions__: - -- Robot location - table 1, table 2 -- Object location - table 1, table 2 -- Object in hand - true, false -- Door state - open, closed - ### Problem 3 ```bash @@ -107,34 +91,24 @@ ros2 run delib_ws_worlds run --ros-args -p problem_number:=3 ``` __Goal__: -Banana on table 2. +Bring bread and butter to the dining table. +Fridge and pantry should be closed at the end. __Initial State__: -Banana on table 1. +Bread is in the pantry, which is closed. +Butter is in the fridge, which is closed. __Available Actions__: - Pick object - (may fail with probability 0.5) - Place object - Move robot - Locations: table 1, table 2, door (will fail if door is closed) - (can also fail while moving with probability 0.1) - Open door - (may fail with probability 0.5) - Close door +- Detect objects (optional) -__Available Conditions__: - -- Robot location - table 1, table 2 -- Object location - table 1, table 2 -- Object in hand - true, false -- Door state - open, closed +Actions may fail with some probability. ### Problem 4 @@ -143,39 +117,28 @@ ros2 run delib_ws_worlds run --ros-args -p problem_number:=4 ``` __Goal__: -Banana on table 2. -Never run out of battery. +Bring bread and butter to the dining table. +Fridge and pantry should be closed at the end. +Waste should be in the dumpster. +Dumpster should be closed. +Don't run out of battery! __Initial State__: -Banana on table 1. +Bread is in the pantry, which is closed. +Butter is in the fridge, which is closed. +Waste is on the office desk and in the office bin. +Hallways into the trash room are closed. __Available Actions__: - Pick object - (may fail with probability 0.5) - Place object - Move robot - Locations: table 1, table 2, door, charging station (will fail if door is closed) - (can also fail while moving with probability 0.1) - Open door - (may fail with probability 0.5) - (returns reason for failure, slipped, locked) - - locked -> find different way - - slipped handle -> try again - Close door -- Charge battery - (can only be done at charging station) - -__Available Conditions__: - -- Robot location - table 1, table 2 -- Object location - table 1, table 2 -- Object in hand - true, false -- Door state - open, closed -- Battery level - (low, normal, full) +- Detect objects (optional) + +Actions may fail with some probability. + +Actions now use up battery, which can be fixed by navigating to the charger. diff --git a/delib_ws_python/delib_ws_python/solutions.py b/delib_ws_python/delib_ws_python/solutions.py index b420e81..d820d8a 100644 --- a/delib_ws_python/delib_ws_python/solutions.py +++ b/delib_ws_python/delib_ws_python/solutions.py @@ -8,30 +8,64 @@ def problem1(): """Gets the Problem 1 solution""" return ( - (ACTIONS.NAVIGATE, "table_source"), - (ACTIONS.PICK, "banana0"), - (ACTIONS.NAVIGATE, "table_sink"), - (ACTIONS.PLACE, "banana0"), + (ACTIONS.NAVIGATE, "pantry_storage"), + (ACTIONS.PICK, "snacks0"), + (ACTIONS.NAVIGATE, "table"), + (ACTIONS.PLACE, "snacks0"), ) def problem2(): """Gets the Problem 2 solution""" return ( - (ACTIONS.NAVIGATE, "hall_banana_farm_dining_room"), - (ACTIONS.OPEN, "hall_banana_farm_dining_room"), - (ACTIONS.NAVIGATE, "table_source"), - (ACTIONS.PICK, "banana0"), - (ACTIONS.NAVIGATE, "table_sink"), - (ACTIONS.PLACE, "banana0"), + (ACTIONS.NAVIGATE, "hall_dining_trash"), + (ACTIONS.OPEN, "hall_dining_trash"), + (ACTIONS.NAVIGATE, "dumpster"), + (ACTIONS.OPEN, "dumpster"), + (ACTIONS.NAVIGATE, "desk"), + (ACTIONS.PICK, "waste0"), + (ACTIONS.NAVIGATE, "dumpster"), + (ACTIONS.PLACE, "waste0"), + (ACTIONS.NAVIGATE, "bin"), + (ACTIONS.PICK, "waste1"), + (ACTIONS.NAVIGATE, "dumpster"), + (ACTIONS.PLACE, "waste1"), + (ACTIONS.CLOSE, "dumpster"), + (ACTIONS.NAVIGATE, "hall_dining_trash"), + (ACTIONS.CLOSE, "hall_dining_trash"), ) def problem3(): """Gets the Problem 3 solution""" - return () # TODO: Implement solution + return ( + (ACTIONS.NAVIGATE, "pantry"), + (ACTIONS.OPEN, "pantry"), + (ACTIONS.DETECT, "bread"), + (ACTIONS.PICK, "bread0"), + (ACTIONS.NAVIGATE, "table"), + (ACTIONS.PLACE, "bread0"), + (ACTIONS.NAVIGATE, "fridge"), + (ACTIONS.OPEN, "fridge"), + (ACTIONS.DETECT, "butter"), + (ACTIONS.PICK, "butter0"), + (ACTIONS.NAVIGATE, "table"), + (ACTIONS.PLACE, "butter0"), + (ACTIONS.NAVIGATE, "fridge"), + (ACTIONS.CLOSE, "fridge"), + (ACTIONS.NAVIGATE, "pantry"), + (ACTIONS.CLOSE, "pantry"), + ) def problem4(): """Gets the Problem 4 solution""" - return () # TODO: Implement solution + return ( + problem3() + + ( + (ACTIONS.NAVIGATE, "hall_dining_closet"), + (ACTIONS.OPEN, "hall_dining_closet"), + (ACTIONS.NAVIGATE, "charger"), + ) + + problem2() + ) diff --git a/delib_ws_worlds/delib_ws_worlds/is_at_goal.py b/delib_ws_worlds/delib_ws_worlds/is_at_goal.py index af4d53a..e7c9186 100644 --- a/delib_ws_worlds/delib_ws_worlds/is_at_goal.py +++ b/delib_ws_worlds/delib_ws_worlds/is_at_goal.py @@ -9,8 +9,23 @@ def get_goal_state(problem_number): """Returns the goal state based on the problem number specified.""" - if problem_number in (1, 2, 3, 4): - return (("objects.banana0.parent", "table_sink_tabletop"),) + if problem_number == 1: + return (("objects.snacks0.parent", "table_tabletop"),) + elif problem_number == 2: + return ( + ("objects.waste0.parent", "dumpster_disposal"), + ("objects.waste1.parent", "dumpster_disposal"), + ("locations.dumpster.is_open", False), + ) + elif problem_number == 3: + return ( + ("objects.bread0.parent", "table_tabletop"), + ("objects.butter0.parent", "table_tabletop"), + ("locations.fridge.is_open", False), + ("locations.pantry.is_open", False), + ) + elif problem_number == 4: + return get_goal_state(2) + get_goal_state(3) else: raise ValueError(f"No goal state for problem number: {problem_number}") diff --git a/delib_ws_worlds/worlds/location_data.yaml b/delib_ws_worlds/worlds/location_data.yaml index 1282330..25318a6 100644 --- a/delib_ws_worlds/worlds/location_data.yaml +++ b/delib_ws_worlds/worlds/location_data.yaml @@ -1,17 +1,104 @@ -############################# -# Example location metadata # -############################# +##################### +# Location metadata # +##################### table: footprint: type: box - dims: [0.6, 0.6] + dims: [1.2, 0.8] height: 0.5 nav_poses: - - [0, -0.4, 1.57] + - [0, 0.65, -1.57] # above + - [0, -0.65, 1.57] # below + - [-0.85, 0, 0.0] # left + - [0.85, 0, 3.14] # right locations: - name: "tabletop" footprint: type: parent - padding: 0.1 + padding: 0.05 color: [0.2, 0.2, 0.2] + +desk: + footprint: + type: box + dims: [0.6, 1.2] + height: 0.5 + nav_poses: + - [-0.5, 0, 0.0] # left + - [0.5, 0, 3.14] # right + locations: + - name: "desktop" + footprint: + type: parent + padding: 0.05 + color: [0.5, 0.2, 0.2] + +storage: + footprint: + type: box + dims: [1.0, 0.6] + height: 0.5 + nav_poses: + - [0, 0.55, -1.57] # above + - [0, -0.55, 1.57] # below + locations: + - name: "storage" + footprint: + type: parent + padding: 0.05 + color: [0.2, 0.5, 0.2] + +trashcan_small: + footprint: + type: circle + radius: 0.25 + height: 0.5 + nav_poses: + - [0, 0.5, -1.57] # above + - [0, -0.5, 1.57] # below + - [-0.5, 0, 0.0] # left + - [0.5, 0, 3.14] # right + locations: + - name: "disposal" + footprint: + type: parent + padding: 0.05 + color: [0.1, 0.1, 0.1] + +trashcan_large: + footprint: + type: circle + radius: 0.525 + height: 0.5 + nav_poses: + - [0, 0.75, -1.57] # above + - [0, -0.75, 1.57] # below + - [-0.75, 0, 0.0] # left + - [0.75, 0, 3.14] # right + locations: + - name: "disposal" + footprint: + type: parent + padding: 0.05 + color: [0.1, 0.1, 0.1] + +charger: + footprint: + type: polygon + coords: + - [-0.5, -0.2] + - [0.5, -0.2] + - [0.5, 0.2] + - [-0.5, 0.2] + height: 0.1 + locations: + - name: "dock" + footprint: + type: parent + nav_poses: + - [0, -0.4, 1.57] + - [-0.7, 0, 0] + - [0, 0.4, -1.57] + - [0.7, 0, 3.14] + color: [0.4, 0.4, 0] diff --git a/delib_ws_worlds/worlds/object_data.yaml b/delib_ws_worlds/worlds/object_data.yaml index 2396f0f..fc6332a 100644 --- a/delib_ws_worlds/worlds/object_data.yaml +++ b/delib_ws_worlds/worlds/object_data.yaml @@ -1,9 +1,33 @@ -########################### -# Example object metadata # -########################### +################### +# Object metadata # +################### -banana: +bread: footprint: type: box - dims: [0.05, 0.2] - color: [0.7, 0.7, 0] + dims: [0.15, 0.2] + color: [0.7, 0.5, 0] + +snacks: + footprint: + type: box + dims: [0.12, 0.15] + color: [0.0, 0.0, 0.6] + +soda: + footprint: + type: box + dims: [0.12, 0.15] + color: [0.8, 0.0, 0.0] + +butter: + footprint: + type: box + dims: [0.1, 0.15] + color: [0.6, 0.6, 0.0] + +waste: + footprint: + type: circle + radius: 0.075 + color: [0.3, 0.5, 0.1] diff --git a/delib_ws_worlds/worlds/world1.yaml b/delib_ws_worlds/worlds/world1.yaml index 0ad2316..f0e43cd 100644 --- a/delib_ws_worlds/worlds/world1.yaml +++ b/delib_ws_worlds/worlds/world1.yaml @@ -1,79 +1,218 @@ # Problem 1 World -# -# __Goal__: -# - Banana on table_sink. -# -# __Initial State__: -# - Banana on table_source. -# -# __Available Actions__: -# - Pick object -# - Place object -# - Move robot -# -# __Available Conditions__: -# - Robot location -# dining_room, banana_farm -# - Object location -# table_sink, table_source -# - Object in hand -# true, false metadata: locations: $PWD/location_data.yaml objects: $PWD/object_data.yaml +params: + name: delib_ws_problem_1 + inflation_radius: 0.01 + object_radius: 0.01 + robots: - name: robot radius: 0.1 - location: dining_room + location: dining path_executor: type: constant_velocity path_planner: type: rrt rrt_star: true + max_connection_dist: 1.0 + rewire_radius: 1.0 + max_time: 3.0 rooms: - - name: banana_farm + - name: dining + footprint: + type: polygon + coords: + - [-1.6, -1.0] + - [1.6, -1.0] + - [1.6, 1.0] + - [-1.6, 1.0] + wall_width: 0.2 + color: [0, 1, 0] + + - name: kitchen footprint: type: polygon coords: - - [-2, 1] - - [-1, 1] - - [-1, -1] - - [-2, -1] + - [-4, -3] + - [-0.5, -3] + - [-0.5, -1.75] + - [-2.25, -1.75] + - [-2.25, -0.5] + - [-4, -0.5] wall_width: 0.2 color: [1, 0, 0] - - name: dining_room + + - name: trash footprint: type: polygon coords: - - [2, 1] - - [1, 1] - - [1, -1] - - [2, -1] + - [-4, 0.5] + - [-2.25, 0.5] + - [-2.25, 1.75] + - [-0.5, 1.75] + - [-0.5, 3] + - [-4, 3] wall_width: 0.2 - color: [0, 1, 0] + color: [0.1, 0.1, 0.1] + + - name: closet + footprint: + type: polygon + coords: + - [0.5, 1.75] + - [2.25, 1.75] + - [2.25, 0.5] + - [4, 0.5] + - [4, 3] + - [0.5, 3] + wall_width: 0.2 + color: [0.7, 0.4, 0.5] + + - name: office + footprint: + type: polygon + coords: + - [0.5, -3] + - [4, -3] + - [4, -0.5] + - [2.25, -0.5] + - [2.25, -1.75] + - [0.5, -1.75] + wall_width: 0.2 + color: [0.0, 0.0, 1.0] hallways: - - room_start: banana_farm - room_end: dining_room - width: 0.3 - conn_method: auto + - room_start: dining + room_end: office + width: 0.6 + conn_method: angle + conn_angle: -1.57 + offset: 1.1 + is_open: true + is_locked: false + + - room_start: dining + room_end: closet + width: 0.6 + conn_method: angle + conn_angle: 1.57 + offset: -1.1 + is_open: true + is_locked: false + + - room_start: dining + room_end: trash + width: 0.6 + conn_method: angle + conn_angle: 1.57 + offset: 1.1 + is_open: true + is_locked: false + + - room_start: dining + room_end: kitchen + width: 0.6 + conn_method: angle + conn_angle: -1.57 + offset: -1.1 + is_open: true + is_locked: false + + - room_start: kitchen + room_end: trash + width: 0.8 + conn_method: angle + conn_angle: 1.57 + offset: 0.6 + is_open: true + is_locked: false + + - room_start: kitchen + room_end: office + width: 0.8 + conn_method: angle + conn_angle: 0.0 + offset: -0.5 + is_open: true + is_locked: false + + - room_start: closet + room_end: trash + width: 0.8 + conn_method: angle + conn_angle: 3.14 + offset: -0.5 + is_open: true + is_locked: false + + - room_start: closet + room_end: office + width: 0.8 + conn_method: angle + conn_angle: -1.57 + offset: 0.6 is_open: true is_locked: false locations: - - name: table_source - parent: banana_farm - category: table - pose: [-1.5, 0.5] - - name: table_sink - parent: dining_room - category: table - pose: [1.5, 0.5] + - name: table + parent: dining + category: table + pose: [0.0, 0.15] + + - name: desk + parent: office + category: desk + pose: [3.6, -2.3] + + - name: fridge + parent: kitchen + category: storage + pose: [-3, -2.65] + is_open: true + + - name: pantry + parent: kitchen + category: storage + pose: [-3.65, -1.5, 0.0, 1.57] + is_open: true + + - name: bin + parent: office + category: trashcan_small + pose: [2.6, -0.85] + + - name: dumpster + parent: trash + category: trashcan_large + pose: [-3.4, 2.4] + + - name: charger + parent: closet + category: charger + pose: [3.5, 2.5, 0, -0.785] + is_charger: true objects: - - parent: table_source - category: banana - pose: [-1.5, 0.5] + - parent: pantry + category: bread + + - parent: pantry + category: snacks + + - parent: fridge + category: soda + + - parent: fridge + category: butter + + - parent: desk + category: waste + + - parent: bin + category: waste diff --git a/delib_ws_worlds/worlds/world2.yaml b/delib_ws_worlds/worlds/world2.yaml index 288ce29..d70c29e 100644 --- a/delib_ws_worlds/worlds/world2.yaml +++ b/delib_ws_worlds/worlds/world2.yaml @@ -1,82 +1,221 @@ # Problem 2 World -# -# __Goal__: -# - Banana on table_sink. -# -# __Initial State__: -# - Banana on table_source. -# - Door closed -# -# __Available Actions__: -# - Pick object -# - Place object -# - Move robot -# - Open door -# - Close door -# -# __Available Conditions__: -# - Robot location -# dining_room, banana_farm -# - Object location -# table_sink, table_source -# - Object in hand -# true, false +# Adds some closed locations metadata: locations: $PWD/location_data.yaml objects: $PWD/object_data.yaml +params: + name: delib_ws_problem_2 + inflation_radius: 0.01 + object_radius: 0.01 + robots: - name: robot radius: 0.1 - location: dining_room + location: dining path_executor: type: constant_velocity path_planner: type: rrt rrt_star: true + max_connection_dist: 1.0 + rewire_radius: 1.0 + max_time: 3.0 rooms: - - name: banana_farm + - name: dining + footprint: + type: polygon + coords: + - [-1.6, -1.0] + - [1.6, -1.0] + - [1.6, 1.0] + - [-1.6, 1.0] + wall_width: 0.2 + color: [0, 1, 0] + + - name: kitchen footprint: type: polygon coords: - - [-2, 1] - - [-1, 1] - - [-1, -1] - - [-2, -1] + - [-4, -3] + - [-0.5, -3] + - [-0.5, -1.75] + - [-2.25, -1.75] + - [-2.25, -0.5] + - [-4, -0.5] wall_width: 0.2 color: [1, 0, 0] - - name: dining_room + + - name: trash footprint: type: polygon coords: - - [2, 1] - - [1, 1] - - [1, -1] - - [2, -1] + - [-4, 0.5] + - [-2.25, 0.5] + - [-2.25, 1.75] + - [-0.5, 1.75] + - [-0.5, 3] + - [-4, 3] wall_width: 0.2 - color: [0, 1, 0] + color: [0.1, 0.1, 0.1] + + - name: closet + footprint: + type: polygon + coords: + - [0.5, 1.75] + - [2.25, 1.75] + - [2.25, 0.5] + - [4, 0.5] + - [4, 3] + - [0.5, 3] + wall_width: 0.2 + color: [0.7, 0.4, 0.5] + + - name: office + footprint: + type: polygon + coords: + - [0.5, -3] + - [4, -3] + - [4, -0.5] + - [2.25, -0.5] + - [2.25, -1.75] + - [0.5, -1.75] + wall_width: 0.2 + color: [0.0, 0.0, 1.0] hallways: - - room_start: banana_farm - room_end: dining_room - width: 0.3 - conn_method: auto + - room_start: dining + room_end: office + width: 0.6 + conn_method: angle + conn_angle: -1.57 + offset: 1.1 + is_open: true + is_locked: false + + - room_start: dining + room_end: closet + width: 0.6 + conn_method: angle + conn_angle: 1.57 + offset: -1.1 + is_open: false + is_locked: false + + - room_start: dining + room_end: trash + width: 0.6 + conn_method: angle + conn_angle: 1.57 + offset: 1.1 + is_open: false + is_locked: false + + - room_start: dining + room_end: kitchen + width: 0.6 + conn_method: angle + conn_angle: -1.57 + offset: -1.1 + is_open: true + is_locked: false + + - room_start: kitchen + room_end: trash + width: 0.8 + conn_method: angle + conn_angle: 1.57 + offset: 0.6 + is_open: false + is_locked: false + + - room_start: kitchen + room_end: office + width: 0.8 + conn_method: angle + conn_angle: 0.0 + offset: -0.5 + is_open: false + is_locked: false + + - room_start: closet + room_end: trash + width: 0.8 + conn_method: angle + conn_angle: 3.14 + offset: -0.5 + is_open: false + is_locked: false + + - room_start: closet + room_end: office + width: 0.8 + conn_method: angle + conn_angle: -1.57 + offset: 0.6 is_open: false is_locked: false locations: - - name: table_source - parent: banana_farm - category: table - pose: [-1.5, 0.5] - - name: table_sink - parent: dining_room - category: table - pose: [1.5, 0.5] + - name: table + parent: dining + category: table + pose: [0.0, 0.15] + + - name: desk + parent: office + category: desk + pose: [3.6, -2.3] + + - name: fridge + parent: kitchen + category: storage + pose: [-3, -2.65] + is_open: false + + - name: pantry + parent: kitchen + category: storage + pose: [-3.65, -1.5, 0.0, 1.57] + is_open: true + + - name: bin + parent: office + category: trashcan_small + pose: [2.6, -0.85] + is_open: true + + - name: dumpster + parent: trash + category: trashcan_large + pose: [-3.4, 2.4] + is_open: false + + - name: charger + parent: closet + category: charger + pose: [3.5, 2.5, 0, -0.785] + is_charger: true objects: - - parent: table_source - category: banana - pose: [-1.5, 0.5] + - parent: pantry + category: bread + + - parent: pantry + category: snacks + + - parent: fridge + category: soda + + - parent: fridge + category: butter + + - parent: desk + category: waste + + - parent: bin + category: waste diff --git a/delib_ws_worlds/worlds/world3.yaml b/delib_ws_worlds/worlds/world3.yaml index df9952a..cb10646 100644 --- a/delib_ws_worlds/worlds/world3.yaml +++ b/delib_ws_worlds/worlds/world3.yaml @@ -1,92 +1,235 @@ # Problem 3 World -# -# __Goal__: -# - Banana on table_sink. -# -# __Initial State__: -# - Banana on table_source. -# - Door closed -# -# __Available Actions__: -# - Pick object -# - Place object -# - Move robot -# - Open door -# - Close door -# -# __Available Conditions__: -# - Robot location -# dining_room, banana_farm -# - Object location -# table_sink, table_source -# - Object in hand -# true, false +# Adds detection and action success probability. metadata: locations: $PWD/location_data.yaml objects: $PWD/object_data.yaml +params: + name: delib_ws_problem_3 + inflation_radius: 0.01 + object_radius: 0.01 + robots: - name: robot radius: 0.1 - location: dining_room + location: dining path_executor: type: constant_velocity path_planner: type: rrt rrt_star: true + max_connection_dist: 1.0 + rewire_radius: 1.0 + max_time: 3.0 action_execution_options: navigate: success_probability: 0.9 rng_seed: 42 - battery_usage: 0 pick: success_probability: 0.5 - battery_usage: 0 place: - battery_usage: 0 + success_probability: 0.5 + open: + success_probability: 0.75 + close: + success_probability: 0.75 + detect: + success_probability: 0.8 rooms: - - name: banana_farm + - name: dining footprint: type: polygon coords: - - [-2, 1] - - [-1, 1] - - [-1, -1] - - [-2, -1] + - [-1.6, -1.0] + - [1.6, -1.0] + - [1.6, 1.0] + - [-1.6, 1.0] + wall_width: 0.2 + color: [0, 1, 0] + + - name: kitchen + footprint: + type: polygon + coords: + - [-4, -3] + - [-0.5, -3] + - [-0.5, -1.75] + - [-2.25, -1.75] + - [-2.25, -0.5] + - [-4, -0.5] wall_width: 0.2 color: [1, 0, 0] - - name: dining_room + + - name: trash footprint: type: polygon coords: - - [2, 1] - - [1, 1] - - [1, -1] - - [2, -1] + - [-4, 0.5] + - [-2.25, 0.5] + - [-2.25, 1.75] + - [-0.5, 1.75] + - [-0.5, 3] + - [-4, 3] wall_width: 0.2 - color: [0, 1, 0] + color: [0.1, 0.1, 0.1] + + - name: closet + footprint: + type: polygon + coords: + - [0.5, 1.75] + - [2.25, 1.75] + - [2.25, 0.5] + - [4, 0.5] + - [4, 3] + - [0.5, 3] + wall_width: 0.2 + color: [0.7, 0.4, 0.5] + + - name: office + footprint: + type: polygon + coords: + - [0.5, -3] + - [4, -3] + - [4, -0.5] + - [2.25, -0.5] + - [2.25, -1.75] + - [0.5, -1.75] + wall_width: 0.2 + color: [0.0, 0.0, 1.0] hallways: - - room_start: banana_farm - room_end: dining_room - width: 0.3 - conn_method: auto + - room_start: dining + room_end: office + width: 0.6 + conn_method: angle + conn_angle: -1.57 + offset: 1.1 + is_open: true + is_locked: false + + - room_start: dining + room_end: closet + width: 0.6 + conn_method: angle + conn_angle: 1.57 + offset: -1.1 + is_open: false + is_locked: false + + - room_start: dining + room_end: trash + width: 0.6 + conn_method: angle + conn_angle: 1.57 + offset: 1.1 + is_open: false + is_locked: false + + - room_start: dining + room_end: kitchen + width: 0.6 + conn_method: angle + conn_angle: -1.57 + offset: -1.1 + is_open: true + is_locked: false + + - room_start: kitchen + room_end: trash + width: 0.8 + conn_method: angle + conn_angle: 1.57 + offset: 0.6 + is_open: false + is_locked: false + + - room_start: kitchen + room_end: office + width: 0.8 + conn_method: angle + conn_angle: 0.0 + offset: -0.5 + is_open: false + is_locked: false + + - room_start: closet + room_end: trash + width: 0.8 + conn_method: angle + conn_angle: 3.14 + offset: -0.5 + is_open: false + is_locked: false + + - room_start: closet + room_end: office + width: 0.8 + conn_method: angle + conn_angle: -1.57 + offset: 0.6 is_open: false is_locked: false locations: - - name: table_source - parent: banana_farm - category: table - pose: [-1.5, 0.5] - - name: table_sink - parent: dining_room - category: table - pose: [1.5, 0.5] + - name: table + parent: dining + category: table + pose: [0.0, 0.15] + + - name: desk + parent: office + category: desk + pose: [3.6, -2.3] + + - name: fridge + parent: kitchen + category: storage + pose: [-3, -2.65] + is_open: false + + - name: pantry + parent: kitchen + category: storage + pose: [-3.65, -1.5, 0.0, 1.57] + is_open: true + + - name: bin + parent: office + category: trashcan_small + pose: [2.6, -0.85] + is_open: true + + - name: dumpster + parent: trash + category: trashcan_large + pose: [-3.4, 2.4] + is_open: false + + - name: charger + parent: closet + category: charger + pose: [3.5, 2.5, 0, -0.785] + is_charger: true objects: - - parent: table_source - category: banana - pose: [-1.5, 0.5] + - parent: pantry + category: bread + + - parent: pantry + category: snacks + + - parent: fridge + category: soda + + - parent: fridge + category: butter + + - parent: desk + category: waste + + - parent: bin + category: waste diff --git a/delib_ws_worlds/worlds/world4.yaml b/delib_ws_worlds/worlds/world4.yaml index e0c9b4a..3c534a9 100644 --- a/delib_ws_worlds/worlds/world4.yaml +++ b/delib_ws_worlds/worlds/world4.yaml @@ -1,102 +1,241 @@ # Problem 4 World -# -# __Goal__: -# - Banana on table_sink. -# -# __Initial State__: -# - Banana on table_source. -# - Door closed -# -# __Available Actions__: -# - Pick object -# - Place object -# - Move robot -# - Open door -# - Close door -# -# __Available Conditions__: -# - Robot location -# dining_room, banana_farm -# - Object location -# table_sink, table_source -# - Object in hand -# true, false +# Adds battery usage to actions. metadata: locations: $PWD/location_data.yaml objects: $PWD/object_data.yaml +params: + name: delib_ws_problem_4 + inflation_radius: 0.01 + object_radius: 0.01 + robots: - name: robot radius: 0.1 - location: dining_room + location: dining path_executor: type: constant_velocity path_planner: type: rrt rrt_star: true + max_connection_dist: 1.0 + rewire_radius: 1.0 + max_time: 3.0 action_execution_options: navigate: success_probability: 0.9 rng_seed: 42 - battery_usage: 1 + battery_usage: 0.5 # Per meter traveled pick: success_probability: 0.5 battery_usage: 5 place: + success_probability: 0.5 battery_usage: 5 open: success_probability: 0.75 - battery_usage: 10 + battery_usage: 8 close: - battery_usage: 10 + success_probability: 0.75 + battery_usage: 8 + detect: + success_probability: 0.8 + battery_usage: 0 rooms: - - name: banana_farm + - name: dining footprint: type: polygon coords: - - [-2, 1] - - [-1, 1] - - [-1, -1] - - [-2, -1] + - [-1.6, -1.0] + - [1.6, -1.0] + - [1.6, 1.0] + - [-1.6, 1.0] + wall_width: 0.2 + color: [0, 1, 0] + + - name: kitchen + footprint: + type: polygon + coords: + - [-4, -3] + - [-0.5, -3] + - [-0.5, -1.75] + - [-2.25, -1.75] + - [-2.25, -0.5] + - [-4, -0.5] wall_width: 0.2 color: [1, 0, 0] - - name: dining_room + + - name: trash footprint: type: polygon coords: - - [2, 1] - - [1, 1] - - [1, -1] - - [2, -1] + - [-4, 0.5] + - [-2.25, 0.5] + - [-2.25, 1.75] + - [-0.5, 1.75] + - [-0.5, 3] + - [-4, 3] wall_width: 0.2 - color: [0, 1, 0] + color: [0.1, 0.1, 0.1] + + - name: closet + footprint: + type: polygon + coords: + - [0.5, 1.75] + - [2.25, 1.75] + - [2.25, 0.5] + - [4, 0.5] + - [4, 3] + - [0.5, 3] + wall_width: 0.2 + color: [0.7, 0.4, 0.5] + + - name: office + footprint: + type: polygon + coords: + - [0.5, -3] + - [4, -3] + - [4, -0.5] + - [2.25, -0.5] + - [2.25, -1.75] + - [0.5, -1.75] + wall_width: 0.2 + color: [0.0, 0.0, 1.0] hallways: - - room_start: banana_farm - room_end: dining_room - width: 0.3 - conn_method: auto + - room_start: dining + room_end: office + width: 0.6 + conn_method: angle + conn_angle: -1.57 + offset: 1.1 + is_open: true + is_locked: false + + - room_start: dining + room_end: closet + width: 0.6 + conn_method: angle + conn_angle: 1.57 + offset: -1.1 + is_open: false + is_locked: false + + - room_start: dining + room_end: trash + width: 0.6 + conn_method: angle + conn_angle: 1.57 + offset: 1.1 + is_open: false + is_locked: false + + - room_start: dining + room_end: kitchen + width: 0.6 + conn_method: angle + conn_angle: -1.57 + offset: -1.1 + is_open: true + is_locked: false + + - room_start: kitchen + room_end: trash + width: 0.8 + conn_method: angle + conn_angle: 1.57 + offset: 0.6 + is_open: false + is_locked: false + + - room_start: kitchen + room_end: office + width: 0.8 + conn_method: angle + conn_angle: 0.0 + offset: -0.5 + is_open: false + is_locked: false + + - room_start: closet + room_end: trash + width: 0.8 + conn_method: angle + conn_angle: 3.14 + offset: -0.5 + is_open: false + is_locked: false + + - room_start: closet + room_end: office + width: 0.8 + conn_method: angle + conn_angle: -1.57 + offset: 0.6 is_open: false is_locked: false locations: - - name: table_source - parent: banana_farm - category: table - pose: [-1.5, 0.5] - - name: table_sink - parent: dining_room - category: table - pose: [1.5, 0.5] - - name: charging_station - parent: dining_room - category: charging_station - pose: [1.5, -0.5] - is_charger: true + - name: table + parent: dining + category: table + pose: [0.0, 0.15] + + - name: desk + parent: office + category: desk + pose: [3.6, -2.3] + + - name: fridge + parent: kitchen + category: storage + pose: [-3, -2.65] + is_open: false + + - name: pantry + parent: kitchen + category: storage + pose: [-3.65, -1.5, 0.0, 1.57] + is_open: true + + - name: bin + parent: office + category: trashcan_small + pose: [2.6, -0.85] + is_open: true + + - name: dumpster + parent: trash + category: trashcan_large + pose: [-3.4, 2.4] + is_open: false + + - name: charger + parent: closet + category: charger + pose: [3.5, 2.5, 0, -0.785] + is_charger: true objects: - - parent: table_source - category: banana - pose: [-1.5, 0.5] + - parent: pantry + category: bread + + - parent: pantry + category: snacks + + - parent: fridge + category: soda + + - parent: fridge + category: butter + + - parent: desk + category: waste + + - parent: bin + category: waste diff --git a/dependencies/pyrobosim b/dependencies/pyrobosim index 549c144..429081d 160000 --- a/dependencies/pyrobosim +++ b/dependencies/pyrobosim @@ -1 +1 @@ -Subproject commit 549c144a029e1fecdea0970d118021355c3cbe70 +Subproject commit 429081d8dc3ff8e3aa67ac5b073ebcb86c7e0880 From 17c8349b60748e9ed1a8e2daa95028aba29a7a19 Mon Sep 17 00:00:00 2001 From: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Date: Wed, 4 Sep 2024 22:44:54 -0400 Subject: [PATCH 22/51] Reorganize repository structure (#20) --- .gitmodules | 18 +++++++++--------- README.md | 4 ++-- clean_build.sh | 5 +++++ dependencies/README.md | 3 +++ dependencies/pyrobosim | 2 +- dependencies/{ => ros_bt_py}/ros2_ros_bt_py | 0 dependencies/{ => ros_bt_py}/rosbridge_suite | 0 problems/README.md | 5 +++++ .../delib_ws_problem_interface}/LICENSE | 0 .../delib_ws_problem_interface/__init__.py | 0 .../perform_action.py | 0 .../delib_ws_problem_interface/world_state.py | 0 .../delib_ws_problem_interface}/package.xml | 0 .../resource/delib_ws_problem_interface | 0 .../delib_ws_problem_interface}/setup.cfg | 0 .../delib_ws_problem_interface}/setup.py | 0 .../test/test_copyright.py | 0 .../test/test_flake8.py | 0 .../test/test_pep257.py | 0 .../delib_ws_python}/LICENSE | 0 .../delib_ws_python/__init__.py | 0 .../delib_ws_python}/delib_ws_python/run.py | 0 .../delib_ws_python/solutions.py | 0 .../delib_ws_python}/launch/run.launch.py | 0 .../delib_ws_python}/package.xml | 0 .../delib_ws_python}/resource/delib_ws_python | 0 .../delib_ws_python}/setup.cfg | 0 .../delib_ws_python}/setup.py | 0 .../delib_ws_python}/test/test_copyright.py | 0 .../delib_ws_python}/test/test_flake8.py | 0 .../delib_ws_python}/test/test_pep257.py | 0 .../delib_ws_worlds/__init__.py | 0 .../delib_ws_worlds/is_at_goal.py | 0 .../delib_ws_worlds}/delib_ws_worlds/run.py | 0 .../delib_ws_worlds}/package.xml | 0 .../delib_ws_worlds}/resource/delib_ws_worlds | 0 .../delib_ws_worlds}/setup.cfg | 0 .../delib_ws_worlds}/setup.py | 0 .../delib_ws_worlds}/test/test_copyright.py | 0 .../delib_ws_worlds}/test/test_flake8.py | 0 .../delib_ws_worlds}/test/test_pep257.py | 0 .../delib_ws_worlds}/worlds/location_data.yaml | 0 .../delib_ws_worlds}/worlds/object_data.yaml | 0 .../delib_ws_worlds}/worlds/world1.yaml | 0 .../delib_ws_worlds}/worlds/world2.yaml | 0 .../delib_ws_worlds}/worlds/world3.yaml | 0 .../delib_ws_worlds}/worlds/world4.yaml | 0 technologies/BehaviorTree.CPP/.gitkeep | 1 + technologies/FlexBE/.gitkeep | 1 + technologies/README.md | 9 +++++++++ .../SkiROS2/skiros2_pyrobosim_lib | 0 technologies/ros_bt_py/.gitkeep | 1 + 52 files changed, 37 insertions(+), 12 deletions(-) create mode 100755 clean_build.sh create mode 100644 dependencies/README.md rename dependencies/{ => ros_bt_py}/ros2_ros_bt_py (100%) rename dependencies/{ => ros_bt_py}/rosbridge_suite (100%) create mode 100644 problems/README.md rename {delib_ws_problem_interface => problems/delib_ws_problem_interface}/LICENSE (100%) rename {delib_ws_problem_interface => problems/delib_ws_problem_interface}/delib_ws_problem_interface/__init__.py (100%) rename {delib_ws_problem_interface => problems/delib_ws_problem_interface}/delib_ws_problem_interface/perform_action.py (100%) rename {delib_ws_problem_interface => problems/delib_ws_problem_interface}/delib_ws_problem_interface/world_state.py (100%) rename {delib_ws_problem_interface => problems/delib_ws_problem_interface}/package.xml (100%) rename {delib_ws_problem_interface => problems/delib_ws_problem_interface}/resource/delib_ws_problem_interface (100%) rename {delib_ws_problem_interface => problems/delib_ws_problem_interface}/setup.cfg (100%) rename {delib_ws_problem_interface => problems/delib_ws_problem_interface}/setup.py (100%) rename {delib_ws_problem_interface => problems/delib_ws_problem_interface}/test/test_copyright.py (100%) rename {delib_ws_problem_interface => problems/delib_ws_problem_interface}/test/test_flake8.py (100%) rename {delib_ws_problem_interface => problems/delib_ws_problem_interface}/test/test_pep257.py (100%) rename {delib_ws_python => problems/delib_ws_python}/LICENSE (100%) rename {delib_ws_python => problems/delib_ws_python}/delib_ws_python/__init__.py (100%) rename {delib_ws_python => problems/delib_ws_python}/delib_ws_python/run.py (100%) rename {delib_ws_python => problems/delib_ws_python}/delib_ws_python/solutions.py (100%) rename {delib_ws_python => problems/delib_ws_python}/launch/run.launch.py (100%) rename {delib_ws_python => problems/delib_ws_python}/package.xml (100%) rename {delib_ws_python => problems/delib_ws_python}/resource/delib_ws_python (100%) rename {delib_ws_python => problems/delib_ws_python}/setup.cfg (100%) rename {delib_ws_python => problems/delib_ws_python}/setup.py (100%) rename {delib_ws_python => problems/delib_ws_python}/test/test_copyright.py (100%) rename {delib_ws_python => problems/delib_ws_python}/test/test_flake8.py (100%) rename {delib_ws_python => problems/delib_ws_python}/test/test_pep257.py (100%) rename {delib_ws_worlds => problems/delib_ws_worlds}/delib_ws_worlds/__init__.py (100%) rename {delib_ws_worlds => problems/delib_ws_worlds}/delib_ws_worlds/is_at_goal.py (100%) rename {delib_ws_worlds => problems/delib_ws_worlds}/delib_ws_worlds/run.py (100%) rename {delib_ws_worlds => problems/delib_ws_worlds}/package.xml (100%) rename {delib_ws_worlds => problems/delib_ws_worlds}/resource/delib_ws_worlds (100%) rename {delib_ws_worlds => problems/delib_ws_worlds}/setup.cfg (100%) rename {delib_ws_worlds => problems/delib_ws_worlds}/setup.py (100%) rename {delib_ws_worlds => problems/delib_ws_worlds}/test/test_copyright.py (100%) rename {delib_ws_worlds => problems/delib_ws_worlds}/test/test_flake8.py (100%) rename {delib_ws_worlds => problems/delib_ws_worlds}/test/test_pep257.py (100%) rename {delib_ws_worlds => problems/delib_ws_worlds}/worlds/location_data.yaml (100%) rename {delib_ws_worlds => problems/delib_ws_worlds}/worlds/object_data.yaml (100%) rename {delib_ws_worlds => problems/delib_ws_worlds}/worlds/world1.yaml (100%) rename {delib_ws_worlds => problems/delib_ws_worlds}/worlds/world2.yaml (100%) rename {delib_ws_worlds => problems/delib_ws_worlds}/worlds/world3.yaml (100%) rename {delib_ws_worlds => problems/delib_ws_worlds}/worlds/world4.yaml (100%) create mode 100644 technologies/BehaviorTree.CPP/.gitkeep create mode 100644 technologies/FlexBE/.gitkeep create mode 100644 technologies/README.md rename {dependencies => technologies}/SkiROS2/skiros2_pyrobosim_lib (100%) create mode 100644 technologies/ros_bt_py/.gitkeep diff --git a/.gitmodules b/.gitmodules index 690d18f..4528c0d 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,24 +1,24 @@ -[submodule "dependencies/pyrobosim"] +[submodule "pyrobosim"] path = dependencies/pyrobosim url = https://github.com/sea-bass/pyrobosim.git branch = main -[submodule "dependencies/SkiROS2/skiros2"] +[submodule "skiros2"] path = dependencies/SkiROS2/skiros2 url = https://github.com/RVMI/skiros2 branch = fix/ros2_jazzy_build -[submodule "dependencies/SkiROS2/skiros2_std_lib"] +[submodule "skiros2_std_lib"] path = dependencies/SkiROS2/skiros2_std_lib url = https://github.com/RVMI/skiros2_std_lib branch = ros2 -[submodule "dependencies/ros2_ros_bt_py"] - path = dependencies/ros2_ros_bt_py +[submodule "ros2_ros_bt_py"] + path = dependencies/ros_bt_py/ros2_ros_bt_py url = https://github.com/fzi-forschungszentrum-informatik/ros2_ros_bt_py.git branch = dev -[submodule "dependencies/rosbridge_suite"] - path = dependencies/rosbridge_suite +[submodule "rosbridge_suite"] + path = dependencies/ros_bt_py/rosbridge_suite url = https://github.com/RobotWebTools/rosbridge_suite.git branch = ros2 -[submodule "dependencies/SkiROS2/skiros2_pyrobosim_lib"] - path = dependencies/SkiROS2/skiros2_pyrobosim_lib +[submodule "skiros2_pyrobosim_lib"] + path = technologies/SkiROS2/skiros2_pyrobosim_lib url = https://github.com/matthias-mayr/skiros2_pyrobosim_lib.git branch = main diff --git a/README.md b/README.md index 6192cc6..95bd18d 100644 --- a/README.md +++ b/README.md @@ -33,10 +33,10 @@ ros2 run delib_ws_p1 run ``` **NOTE:** If you want to cleanup the colcon build artifacts across container usage, -you can run this command: +you can run this command (it will ask you for your sudo password): ```bash -sudo rm -rf .colcon/build .colcon/install .colcon/log +./clean_build.sh ``` --- diff --git a/clean_build.sh b/clean_build.sh new file mode 100755 index 0000000..fda2a93 --- /dev/null +++ b/clean_build.sh @@ -0,0 +1,5 @@ +#!/bin/sh +# Helper script to clean up the build artifacts generated inside the Docker container. +# It would be nice to not use root user inside the container, but this is not yet in place. + +sudo rm -rf .colcon/build .colcon/install .colcon/log diff --git a/dependencies/README.md b/dependencies/README.md new file mode 100644 index 0000000..9b3a4c5 --- /dev/null +++ b/dependencies/README.md @@ -0,0 +1,3 @@ +## Dependencies + +This contains all the external dependencies that must be built from source. diff --git a/dependencies/pyrobosim b/dependencies/pyrobosim index 429081d..ce53680 160000 --- a/dependencies/pyrobosim +++ b/dependencies/pyrobosim @@ -1 +1 @@ -Subproject commit 429081d8dc3ff8e3aa67ac5b073ebcb86c7e0880 +Subproject commit ce53680d315181161cb86bfb6b6f28d6f7769a41 diff --git a/dependencies/ros2_ros_bt_py b/dependencies/ros_bt_py/ros2_ros_bt_py similarity index 100% rename from dependencies/ros2_ros_bt_py rename to dependencies/ros_bt_py/ros2_ros_bt_py diff --git a/dependencies/rosbridge_suite b/dependencies/ros_bt_py/rosbridge_suite similarity index 100% rename from dependencies/rosbridge_suite rename to dependencies/ros_bt_py/rosbridge_suite diff --git a/problems/README.md b/problems/README.md new file mode 100644 index 0000000..f23c5c3 --- /dev/null +++ b/problems/README.md @@ -0,0 +1,5 @@ +## Problems + +Contains all the initial files for the workshop. + +This includes the world models in [PyRoboSim](https://github.com/sea-bass/pyrobosim), and some simple Python abstractions for commanding simulated robots and retrieving their state. diff --git a/delib_ws_problem_interface/LICENSE b/problems/delib_ws_problem_interface/LICENSE similarity index 100% rename from delib_ws_problem_interface/LICENSE rename to problems/delib_ws_problem_interface/LICENSE diff --git a/delib_ws_problem_interface/delib_ws_problem_interface/__init__.py b/problems/delib_ws_problem_interface/delib_ws_problem_interface/__init__.py similarity index 100% rename from delib_ws_problem_interface/delib_ws_problem_interface/__init__.py rename to problems/delib_ws_problem_interface/delib_ws_problem_interface/__init__.py diff --git a/delib_ws_problem_interface/delib_ws_problem_interface/perform_action.py b/problems/delib_ws_problem_interface/delib_ws_problem_interface/perform_action.py similarity index 100% rename from delib_ws_problem_interface/delib_ws_problem_interface/perform_action.py rename to problems/delib_ws_problem_interface/delib_ws_problem_interface/perform_action.py diff --git a/delib_ws_problem_interface/delib_ws_problem_interface/world_state.py b/problems/delib_ws_problem_interface/delib_ws_problem_interface/world_state.py similarity index 100% rename from delib_ws_problem_interface/delib_ws_problem_interface/world_state.py rename to problems/delib_ws_problem_interface/delib_ws_problem_interface/world_state.py diff --git a/delib_ws_problem_interface/package.xml b/problems/delib_ws_problem_interface/package.xml similarity index 100% rename from delib_ws_problem_interface/package.xml rename to problems/delib_ws_problem_interface/package.xml diff --git a/delib_ws_problem_interface/resource/delib_ws_problem_interface b/problems/delib_ws_problem_interface/resource/delib_ws_problem_interface similarity index 100% rename from delib_ws_problem_interface/resource/delib_ws_problem_interface rename to problems/delib_ws_problem_interface/resource/delib_ws_problem_interface diff --git a/delib_ws_problem_interface/setup.cfg b/problems/delib_ws_problem_interface/setup.cfg similarity index 100% rename from delib_ws_problem_interface/setup.cfg rename to problems/delib_ws_problem_interface/setup.cfg diff --git a/delib_ws_problem_interface/setup.py b/problems/delib_ws_problem_interface/setup.py similarity index 100% rename from delib_ws_problem_interface/setup.py rename to problems/delib_ws_problem_interface/setup.py diff --git a/delib_ws_problem_interface/test/test_copyright.py b/problems/delib_ws_problem_interface/test/test_copyright.py similarity index 100% rename from delib_ws_problem_interface/test/test_copyright.py rename to problems/delib_ws_problem_interface/test/test_copyright.py diff --git a/delib_ws_problem_interface/test/test_flake8.py b/problems/delib_ws_problem_interface/test/test_flake8.py similarity index 100% rename from delib_ws_problem_interface/test/test_flake8.py rename to problems/delib_ws_problem_interface/test/test_flake8.py diff --git a/delib_ws_problem_interface/test/test_pep257.py b/problems/delib_ws_problem_interface/test/test_pep257.py similarity index 100% rename from delib_ws_problem_interface/test/test_pep257.py rename to problems/delib_ws_problem_interface/test/test_pep257.py diff --git a/delib_ws_python/LICENSE b/problems/delib_ws_python/LICENSE similarity index 100% rename from delib_ws_python/LICENSE rename to problems/delib_ws_python/LICENSE diff --git a/delib_ws_python/delib_ws_python/__init__.py b/problems/delib_ws_python/delib_ws_python/__init__.py similarity index 100% rename from delib_ws_python/delib_ws_python/__init__.py rename to problems/delib_ws_python/delib_ws_python/__init__.py diff --git a/delib_ws_python/delib_ws_python/run.py b/problems/delib_ws_python/delib_ws_python/run.py similarity index 100% rename from delib_ws_python/delib_ws_python/run.py rename to problems/delib_ws_python/delib_ws_python/run.py diff --git a/delib_ws_python/delib_ws_python/solutions.py b/problems/delib_ws_python/delib_ws_python/solutions.py similarity index 100% rename from delib_ws_python/delib_ws_python/solutions.py rename to problems/delib_ws_python/delib_ws_python/solutions.py diff --git a/delib_ws_python/launch/run.launch.py b/problems/delib_ws_python/launch/run.launch.py similarity index 100% rename from delib_ws_python/launch/run.launch.py rename to problems/delib_ws_python/launch/run.launch.py diff --git a/delib_ws_python/package.xml b/problems/delib_ws_python/package.xml similarity index 100% rename from delib_ws_python/package.xml rename to problems/delib_ws_python/package.xml diff --git a/delib_ws_python/resource/delib_ws_python b/problems/delib_ws_python/resource/delib_ws_python similarity index 100% rename from delib_ws_python/resource/delib_ws_python rename to problems/delib_ws_python/resource/delib_ws_python diff --git a/delib_ws_python/setup.cfg b/problems/delib_ws_python/setup.cfg similarity index 100% rename from delib_ws_python/setup.cfg rename to problems/delib_ws_python/setup.cfg diff --git a/delib_ws_python/setup.py b/problems/delib_ws_python/setup.py similarity index 100% rename from delib_ws_python/setup.py rename to problems/delib_ws_python/setup.py diff --git a/delib_ws_python/test/test_copyright.py b/problems/delib_ws_python/test/test_copyright.py similarity index 100% rename from delib_ws_python/test/test_copyright.py rename to problems/delib_ws_python/test/test_copyright.py diff --git a/delib_ws_python/test/test_flake8.py b/problems/delib_ws_python/test/test_flake8.py similarity index 100% rename from delib_ws_python/test/test_flake8.py rename to problems/delib_ws_python/test/test_flake8.py diff --git a/delib_ws_python/test/test_pep257.py b/problems/delib_ws_python/test/test_pep257.py similarity index 100% rename from delib_ws_python/test/test_pep257.py rename to problems/delib_ws_python/test/test_pep257.py diff --git a/delib_ws_worlds/delib_ws_worlds/__init__.py b/problems/delib_ws_worlds/delib_ws_worlds/__init__.py similarity index 100% rename from delib_ws_worlds/delib_ws_worlds/__init__.py rename to problems/delib_ws_worlds/delib_ws_worlds/__init__.py diff --git a/delib_ws_worlds/delib_ws_worlds/is_at_goal.py b/problems/delib_ws_worlds/delib_ws_worlds/is_at_goal.py similarity index 100% rename from delib_ws_worlds/delib_ws_worlds/is_at_goal.py rename to problems/delib_ws_worlds/delib_ws_worlds/is_at_goal.py diff --git a/delib_ws_worlds/delib_ws_worlds/run.py b/problems/delib_ws_worlds/delib_ws_worlds/run.py similarity index 100% rename from delib_ws_worlds/delib_ws_worlds/run.py rename to problems/delib_ws_worlds/delib_ws_worlds/run.py diff --git a/delib_ws_worlds/package.xml b/problems/delib_ws_worlds/package.xml similarity index 100% rename from delib_ws_worlds/package.xml rename to problems/delib_ws_worlds/package.xml diff --git a/delib_ws_worlds/resource/delib_ws_worlds b/problems/delib_ws_worlds/resource/delib_ws_worlds similarity index 100% rename from delib_ws_worlds/resource/delib_ws_worlds rename to problems/delib_ws_worlds/resource/delib_ws_worlds diff --git a/delib_ws_worlds/setup.cfg b/problems/delib_ws_worlds/setup.cfg similarity index 100% rename from delib_ws_worlds/setup.cfg rename to problems/delib_ws_worlds/setup.cfg diff --git a/delib_ws_worlds/setup.py b/problems/delib_ws_worlds/setup.py similarity index 100% rename from delib_ws_worlds/setup.py rename to problems/delib_ws_worlds/setup.py diff --git a/delib_ws_worlds/test/test_copyright.py b/problems/delib_ws_worlds/test/test_copyright.py similarity index 100% rename from delib_ws_worlds/test/test_copyright.py rename to problems/delib_ws_worlds/test/test_copyright.py diff --git a/delib_ws_worlds/test/test_flake8.py b/problems/delib_ws_worlds/test/test_flake8.py similarity index 100% rename from delib_ws_worlds/test/test_flake8.py rename to problems/delib_ws_worlds/test/test_flake8.py diff --git a/delib_ws_worlds/test/test_pep257.py b/problems/delib_ws_worlds/test/test_pep257.py similarity index 100% rename from delib_ws_worlds/test/test_pep257.py rename to problems/delib_ws_worlds/test/test_pep257.py diff --git a/delib_ws_worlds/worlds/location_data.yaml b/problems/delib_ws_worlds/worlds/location_data.yaml similarity index 100% rename from delib_ws_worlds/worlds/location_data.yaml rename to problems/delib_ws_worlds/worlds/location_data.yaml diff --git a/delib_ws_worlds/worlds/object_data.yaml b/problems/delib_ws_worlds/worlds/object_data.yaml similarity index 100% rename from delib_ws_worlds/worlds/object_data.yaml rename to problems/delib_ws_worlds/worlds/object_data.yaml diff --git a/delib_ws_worlds/worlds/world1.yaml b/problems/delib_ws_worlds/worlds/world1.yaml similarity index 100% rename from delib_ws_worlds/worlds/world1.yaml rename to problems/delib_ws_worlds/worlds/world1.yaml diff --git a/delib_ws_worlds/worlds/world2.yaml b/problems/delib_ws_worlds/worlds/world2.yaml similarity index 100% rename from delib_ws_worlds/worlds/world2.yaml rename to problems/delib_ws_worlds/worlds/world2.yaml diff --git a/delib_ws_worlds/worlds/world3.yaml b/problems/delib_ws_worlds/worlds/world3.yaml similarity index 100% rename from delib_ws_worlds/worlds/world3.yaml rename to problems/delib_ws_worlds/worlds/world3.yaml diff --git a/delib_ws_worlds/worlds/world4.yaml b/problems/delib_ws_worlds/worlds/world4.yaml similarity index 100% rename from delib_ws_worlds/worlds/world4.yaml rename to problems/delib_ws_worlds/worlds/world4.yaml diff --git a/technologies/BehaviorTree.CPP/.gitkeep b/technologies/BehaviorTree.CPP/.gitkeep new file mode 100644 index 0000000..77770c0 --- /dev/null +++ b/technologies/BehaviorTree.CPP/.gitkeep @@ -0,0 +1 @@ +TODO(facontidavide): Remove this file when implementations are available! diff --git a/technologies/FlexBE/.gitkeep b/technologies/FlexBE/.gitkeep new file mode 100644 index 0000000..645e39d --- /dev/null +++ b/technologies/FlexBE/.gitkeep @@ -0,0 +1 @@ +TODO(dcconner): Remove this file when implementations are available! diff --git a/technologies/README.md b/technologies/README.md new file mode 100644 index 0000000..e3af149 --- /dev/null +++ b/technologies/README.md @@ -0,0 +1,9 @@ +## Technologies + +Contains all the technology-specific implementations of ROS 2 Deliberation technologies. + +The technologies in this workshop include: +* [BehaviorTree.CPP](https://github.com/BehaviorTree/BehaviorTree.CPP) - A C++ based library for behavior trees. +* [ros_bt_py](https://github.com/fzi-forschungszentrum-informatik/ros2_ros_bt_py) - A Python based library for behavior trees. +* [FlexBE](https://github.com/FlexBE) - A Python based library for hierarchical finite-state machines. +* [SkiROS2](https://github.com/RVMI/skiros2) - A Python based library for implementing abstract skill interfaces and performing task planning. diff --git a/dependencies/SkiROS2/skiros2_pyrobosim_lib b/technologies/SkiROS2/skiros2_pyrobosim_lib similarity index 100% rename from dependencies/SkiROS2/skiros2_pyrobosim_lib rename to technologies/SkiROS2/skiros2_pyrobosim_lib diff --git a/technologies/ros_bt_py/.gitkeep b/technologies/ros_bt_py/.gitkeep new file mode 100644 index 0000000..a0e5c93 --- /dev/null +++ b/technologies/ros_bt_py/.gitkeep @@ -0,0 +1 @@ +TODO(Oberacda): Remove this file when implementations are available! From 1d93c1b9ef7db20a697200a176f8d45e58ba7999 Mon Sep 17 00:00:00 2001 From: dcconner Date: Thu, 5 Sep 2024 15:45:12 -0400 Subject: [PATCH 23/51] add beta of flexbe behaviors (#21) * add FlexBE dependencies --------- Co-authored-by: Sebastian Castro Co-authored-by: David Conner --- .docker/Dockerfile | 9 +- .gitignore | 1 + .gitmodules | 8 + .pre-commit-config.yaml | 1 + dependencies/FlexBE/flexbe_behavior_engine | 1 + dependencies/FlexBE/flexbe_webui | 1 + docker-compose.yaml | 4 +- python-requirements.txt | 5 +- technologies/FlexBE/CHANGELOG.rst | 7 + technologies/FlexBE/CONTRIBUTING.md | 3 + technologies/FlexBE/LICENSE | 13 + technologies/FlexBE/README.md | 64 +++++ technologies/FlexBE/ament_pycodestyle.ini | 4 + technologies/FlexBE/docs/installation.md | 73 ++++++ .../pyrobosim_flexbe_behaviors/CHANGELOG.rst | 7 + .../pyrobosim_flexbe_behaviors/CMakeLists.txt | 26 ++ .../bin/copy_behavior | 100 +++++++ .../config/example.yaml | 1 + .../manifest/delib_ws_p2.xml | 30 +++ .../manifest/delib_ws_p2_sm.xml | 28 ++ .../manifest/detectselect.xml | 27 ++ .../manifest/test_navigate.xml | 18 ++ .../manifest/test_pick_place.xml | 28 ++ .../manifest/test_plan_path.xml | 18 ++ .../pyrobosim_flexbe_behaviors/package.xml | 33 +++ .../pyrobosim_flexbe_behaviors/__init__.py | 0 .../delib_ws_p2_sm.py | 172 ++++++++++++ .../delib_ws_p2_sm_sm.py | 246 ++++++++++++++++++ .../detectselect_sm.py | 234 +++++++++++++++++ .../test_navigate_sm.py | 187 +++++++++++++ .../test_pick_place_sm.py | 241 +++++++++++++++++ .../test_plan_path_sm.py | 144 ++++++++++ .../resource/pyrobosim_flexbe_behaviors | 0 .../pyrobosim_flexbe_behaviors/setup.cfg | 4 + .../pyrobosim_flexbe_behaviors/setup.py | 27 ++ .../pyrobosim_flexbe_states/CHANGELOG.rst | 7 + .../pyrobosim_flexbe_states/package.xml | 35 +++ .../pyrobosim_flexbe_states/__init__.py | 0 .../detect_local_objects_state.py | 241 +++++++++++++++++ .../detect_objects_state.py | 184 +++++++++++++ .../door_action_state.py | 173 ++++++++++++ .../follow_path_state.py | 165 ++++++++++++ .../navigate_action_state.py | 178 +++++++++++++ .../pick_action_state.py | 166 ++++++++++++ .../place_action_state.py | 160 ++++++++++++ .../plan_path_state.py | 177 +++++++++++++ .../resource/pyrobosim_flexbe_states | 0 .../FlexBE/pyrobosim_flexbe_states/setup.cfg | 4 + .../FlexBE/pyrobosim_flexbe_states/setup.py | 34 +++ .../tests/launch_test.py | 64 +++++ .../tests/run_colcon_test.py | 51 ++++ 51 files changed, 3398 insertions(+), 6 deletions(-) create mode 160000 dependencies/FlexBE/flexbe_behavior_engine create mode 160000 dependencies/FlexBE/flexbe_webui create mode 100644 technologies/FlexBE/CHANGELOG.rst create mode 100644 technologies/FlexBE/CONTRIBUTING.md create mode 100644 technologies/FlexBE/LICENSE create mode 100644 technologies/FlexBE/README.md create mode 100644 technologies/FlexBE/ament_pycodestyle.ini create mode 100644 technologies/FlexBE/docs/installation.md create mode 100644 technologies/FlexBE/pyrobosim_flexbe_behaviors/CHANGELOG.rst create mode 100644 technologies/FlexBE/pyrobosim_flexbe_behaviors/CMakeLists.txt create mode 100755 technologies/FlexBE/pyrobosim_flexbe_behaviors/bin/copy_behavior create mode 100644 technologies/FlexBE/pyrobosim_flexbe_behaviors/config/example.yaml create mode 100644 technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/delib_ws_p2.xml create mode 100644 technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/delib_ws_p2_sm.xml create mode 100644 technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/detectselect.xml create mode 100644 technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/test_navigate.xml create mode 100644 technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/test_pick_place.xml create mode 100644 technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/test_plan_path.xml create mode 100644 technologies/FlexBE/pyrobosim_flexbe_behaviors/package.xml create mode 100644 technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/__init__.py create mode 100644 technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/delib_ws_p2_sm.py create mode 100644 technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/delib_ws_p2_sm_sm.py create mode 100644 technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/detectselect_sm.py create mode 100644 technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/test_navigate_sm.py create mode 100644 technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/test_pick_place_sm.py create mode 100644 technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/test_plan_path_sm.py create mode 100644 technologies/FlexBE/pyrobosim_flexbe_behaviors/resource/pyrobosim_flexbe_behaviors create mode 100644 technologies/FlexBE/pyrobosim_flexbe_behaviors/setup.cfg create mode 100644 technologies/FlexBE/pyrobosim_flexbe_behaviors/setup.py create mode 100644 technologies/FlexBE/pyrobosim_flexbe_states/CHANGELOG.rst create mode 100644 technologies/FlexBE/pyrobosim_flexbe_states/package.xml create mode 100644 technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/__init__.py create mode 100644 technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/detect_local_objects_state.py create mode 100644 technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/detect_objects_state.py create mode 100644 technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/door_action_state.py create mode 100644 technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/follow_path_state.py create mode 100644 technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/navigate_action_state.py create mode 100644 technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/pick_action_state.py create mode 100644 technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/place_action_state.py create mode 100644 technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/plan_path_state.py create mode 100644 technologies/FlexBE/pyrobosim_flexbe_states/resource/pyrobosim_flexbe_states create mode 100644 technologies/FlexBE/pyrobosim_flexbe_states/setup.cfg create mode 100644 technologies/FlexBE/pyrobosim_flexbe_states/setup.py create mode 100644 technologies/FlexBE/pyrobosim_flexbe_states/tests/launch_test.py create mode 100644 technologies/FlexBE/pyrobosim_flexbe_states/tests/run_colcon_test.py diff --git a/.docker/Dockerfile b/.docker/Dockerfile index 91fb574..fef114b 100644 --- a/.docker/Dockerfile +++ b/.docker/Dockerfile @@ -10,19 +10,20 @@ RUN apt-get update && \ apt-get install -y \ apt-utils python3-pip python3-tk python3-wrapt python3-inflection \ libegl1 libgl1-mesa-dev libglu1-mesa-dev '^libxcb.*-dev' libx11-xcb-dev \ - libxi-dev libxkbcommon-dev libxkbcommon-x11-dev libxrender-dev + libxi-dev libxkbcommon-dev libxkbcommon-x11-dev libxrender-dev \ + libnss3 libasound2t64 libxkbfile1 # Create a ROS 2 workspace. RUN mkdir -p /delib_ws/src/ # Install external dependencies. WORKDIR /delib_ws -COPY dependencies src/dependencies -RUN source /opt/ros/jazzy/setup.bash && \ - rosdep install --from-paths src -y --ignore-src # Aggregated Python dependencies COPY python-requirements.txt /delib_ws RUN pip3 install --break-system-packages -r python-requirements.txt +COPY dependencies src/dependencies +RUN source /opt/ros/jazzy/setup.bash && \ + rosdep install --from-paths src -y --ignore-src # SkiROS2 dependencies RUN src/dependencies/SkiROS2/skiros2/skiros2/scripts/install_fd_task_planner.sh diff --git a/.gitignore b/.gitignore index cfa608b..45436f6 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,3 @@ __pycache__/ .colcon/ +.vscode diff --git a/.gitmodules b/.gitmodules index 4528c0d..fb4249a 100644 --- a/.gitmodules +++ b/.gitmodules @@ -22,3 +22,11 @@ path = technologies/SkiROS2/skiros2_pyrobosim_lib url = https://github.com/matthias-mayr/skiros2_pyrobosim_lib.git branch = main +[submodule "dependencies/FlexBE/flexbe_behavior_engine"] + path = dependencies/FlexBE/flexbe_behavior_engine + url = https://github.com/FlexBE/flexbe_behavior_engine.git + branch = ros2-devel +[submodule "dependencies/FlexBE/flexbe_webui"] + path = dependencies/FlexBE/flexbe_webui + url = https://github.com/FlexBE/flexbe_webui.git + branch = main diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 46d7a9c..241e4f8 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -33,6 +33,7 @@ repos: rev: 24.8.0 hooks: - id: black + exclude: ^technologies/FlexBE # Finds spelling issues in code. - repo: https://github.com/codespell-project/codespell diff --git a/dependencies/FlexBE/flexbe_behavior_engine b/dependencies/FlexBE/flexbe_behavior_engine new file mode 160000 index 0000000..f176c03 --- /dev/null +++ b/dependencies/FlexBE/flexbe_behavior_engine @@ -0,0 +1 @@ +Subproject commit f176c0305f54643ac5123841dadae66ebbf4872e diff --git a/dependencies/FlexBE/flexbe_webui b/dependencies/FlexBE/flexbe_webui new file mode 160000 index 0000000..f18cece --- /dev/null +++ b/dependencies/FlexBE/flexbe_webui @@ -0,0 +1 @@ +Subproject commit f18cece1f85ab126370db042958ba1beeebab474 diff --git a/docker-compose.yaml b/docker-compose.yaml index 1eff663..93b9dda 100644 --- a/docker-compose.yaml +++ b/docker-compose.yaml @@ -27,11 +27,13 @@ services: # Networking and IPC for ROS 2 network_mode: host ipc: host - # Allows graphical programs in the container environment: + # Allows graphical programs in the container - DISPLAY=${DISPLAY} - QT_X11_NO_MITSHM=1 - NVIDIA_DRIVER_CAPABILITIES=all + # Enables FlexBE web UI to work as root inside the container + - QTWEBENGINE_DISABLE_SANDBOX=1 volumes: # Mount the workshop source code - ./:/delib_ws/src/:rw diff --git a/python-requirements.txt b/python-requirements.txt index 48cf852..1fa147c 100644 --- a/python-requirements.txt +++ b/python-requirements.txt @@ -1,11 +1,14 @@ adjustText astar +fastAPI==0.89.1 matplotlib numpy<2.1.0 pycollada -PySide6>=6.4.0 +pydantic>=1.10.13 +PySide6>=6.7.1 PyYAML scipy shapely>=2.0.1 transforms3d trimesh +websockets>=10.3 diff --git a/technologies/FlexBE/CHANGELOG.rst b/technologies/FlexBE/CHANGELOG.rst new file mode 100644 index 0000000..9bb76dd --- /dev/null +++ b/technologies/FlexBE/CHANGELOG.rst @@ -0,0 +1,7 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package flexbe_roscon24 +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.0.1 (2024-09-04) +------------------ +* Beta release of ROS 2 FlexBE project for ROSCON 2024 deliberation workshop diff --git a/technologies/FlexBE/CONTRIBUTING.md b/technologies/FlexBE/CONTRIBUTING.md new file mode 100644 index 0000000..0703ae9 --- /dev/null +++ b/technologies/FlexBE/CONTRIBUTING.md @@ -0,0 +1,3 @@ +Any new contributions that you make to this repository will +be under the Apache 2.0 License, as dictated by that +[license](http://www.apache.org/licenses/LICENSE-2.0). diff --git a/technologies/FlexBE/LICENSE b/technologies/FlexBE/LICENSE new file mode 100644 index 0000000..d809660 --- /dev/null +++ b/technologies/FlexBE/LICENSE @@ -0,0 +1,13 @@ +Copyright 2023 Philipp Schillinger, Christopher Newport University + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. diff --git a/technologies/FlexBE/README.md b/technologies/FlexBE/README.md new file mode 100644 index 0000000..0479136 --- /dev/null +++ b/technologies/FlexBE/README.md @@ -0,0 +1,64 @@ +# FlexBE States and Behaviors for ROSCon 2024 Deliberation Technologies Workshop + +> NOTE: This is a very preliminary version + + +# 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](docs/installation.md). + + +## Usage + +We provide a few behaviors, for now use the `delib_ws_p2_sm` behavior. +This includes a statemachine with on nested sub-statemachine to allow selection of a door to open, and a +nested behavior called `DetectSelect` that plans path to location, retrieves a user selected object, and places at target location. + + +To run start the following in separate terminals +* `pyrobosim` with whatever world you choose + * `clear; ros2 run delib_ws_worlds run --ros-args -p problem_number:=2` + +* `clear; ros2 launch flexbe_onboard behavior_onboard.launch.py` + * This is what would be run "onboard" the robot + +* `clear; ros2 launch flexbe_webui flexbe_ocs.launch.py headless:=true` + * This launches the remote "Operator Control Station" (OCS) minus the UI + +* `clear; ros2 run flexbe_webui webui_client` + * This launches UI + * Note: It is best to give the OCS software several seconds to start and load behaviors before launching the UI + Wait on the `Begin behavior mirror processing ...` message in terminal + +* `clear; ros2 run flexbe_input input_action_server` + * This launches an action server that will pop up a simple UI that will allow the operator input data on request + * In this demonstration, it is used to select from among the detected objects + * This must be started to avoid a `'flexbe/behavior_input' is not available` warning in the demonstration behavior. + + From the FlexBE UI "Behavior Dashboard", select "Load Behavior" from the main toolbar, and then select `delib_ws_p2_sm` from the list of available behaviors. + + Click on the "StateMachine Editor" tab to see the state machine. + For now, this demonstration behavior is configured to require the operator to confirm most transitions unless you place in "Full" autonomy on the "Runtime Control". + + Click on the "Runtime Control" tab. + You may specify the parameters for the initial "Move to Location" where you will look for objects, and the "Place Location". + The default values show are specified as "Behavior Parameters" on the dashboard. + + Select "Start Behavior" and follow along as the UI "mirrors" the onboard behavior. + For `p2`, you should first invoke the `open` outcome to activate the "OpenDoorSM" sub-state machine + to open the required doors and dumpster (you may invoke this repeatedly), then + activate the `go` outcome to activate the "DetectSelect" sub-behavior to go to the target + location and detect available objects. + + > Note: In `Low` autonomy, you will need to click on the outcome label when requested (that is, when the state has finished) to confirm the transition when prompted at the UI. + + > WARNING: An operator can preempt a state, so wait for the state to finish and request the outcome by highlighting the transition. + + > NOTE: Much more to follow. + +## To dos + +- [ ] Battery monitor state and concurrency example +- [ ] Define (sub-)behaviors for workshop tasks +- [ ] Write up more detail usage instructions diff --git a/technologies/FlexBE/ament_pycodestyle.ini b/technologies/FlexBE/ament_pycodestyle.ini new file mode 100644 index 0000000..f1bedec --- /dev/null +++ b/technologies/FlexBE/ament_pycodestyle.ini @@ -0,0 +1,4 @@ +[pycodestyle] +ignore = '',E221,W503 +max-line-length = 130 +# For W503 - https://www.flake8rules.com/rules/W503.html and https://peps.python.org/pep-0008/#should-a-line-break-before-or-after-a-binary-operator diff --git a/technologies/FlexBE/docs/installation.md b/technologies/FlexBE/docs/installation.md new file mode 100644 index 0000000..4bb2af0 --- /dev/null +++ b/technologies/FlexBE/docs/installation.md @@ -0,0 +1,73 @@ +# FlexBE States and Behaviors for ROSCon 2024 Deliberation Technologies Workshop + +# Installation and Setup + +This documents installation of the required FlexBE Behavior Engine and FlexBE Webui. +This is provided for the workshop in a Docker container, but are available open source + + - `git clone -b ros2-devel https://github.com/FlexBE/flexbe_behavior_engine.git` + - `git clone -b main https://github.com/FlexBE/flexbe_webui.git` + +This workshop is using the latest `4.0.0+` version of the FlexBE Behavior Engine and WebUI. +> Note: This version does NOT work with the older flexbe_app! + +The WebUI requires the following Python libraries specified in the `requires.txt` file: +``` +PySide6>=6.7.1 +fastAPI==0.89.1 +pydantic>=1.10.13 +setuptools +websockets>=10.3 +``` +On Ubuntu 24.04 (i.e., for ROS 2 Jazzy), the system installs for `sudo apt install python3-websockets python3-pydantic python3-fastapi` +are sufficient and can be installed via `rosdep` from the `package.xml` dependencies. + +Unfortunately, `PySide6` is not in the current Ubuntu 24.04 binaries. +In 24.04 you are required to set up a virtual environment and cannot do a local install. + +The following has been tested under 24.04. First go to your `WORKSPACE_ROOT` (e.g. `ros2_ws`) and run the following commands. +``` +virtualenv -p python3 --system-site-packages ./venv +source ./venv/bin/activate +touch ./venv/COLCON_IGNORE +cd src/ +``` +This creates a `venv` folder that we will `COLCON_IGNORE` during builds. + +We add the following to our `.bashrc` to enable running the `webui_client` from any terminal: + +```bash +echo "Add Python virtual environment to current PYTHONPATH" +VENV_PATH="$WORKSPACE_ROOT/venv/lib/python3.12/site-packages" +if [[ ":$PYTHONPATH:" != *":$VENV_PATH:"* ]]; then + export PYTHONPATH="$PYTHONPATH:$VENV_PATH" +fi +``` + +The demonstrations make use of pyrobosim + - `git clone -b main https://github.com/sea-bass/pyrobosim.git` + + Follow the pyrobosim startup [directions](https://pyrobosim.readthedocs.io/en/latest/) + +> NOTE: Thus far, it has been tested with the `pyrobosim_ros/examples/demo.py` with partial observability enabled: + +```python +-- a/pyrobosim_ros/examples/demo.py ++++ b/pyrobosim_ros/examples/demo.py +@@ -98,6 +98,7 @@ def create_world(): + radius=0.1, + path_executor=ConstantVelocityExecutor(), + path_planner=path_planner, ++ partial_observability=True + ) + +``` + + +This repo provides the following packages: +- `pyrobosim_flexbe_states` + * generic state implementations that interact with the ROS 2 action interfaces provided by pyrobosim. +- `pyrobosim_flexbe_behaviors` + * finite state machines that realize specific behaviors. + +Build all of these packages using standard ROS 2 `colcon build` diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/CHANGELOG.rst b/technologies/FlexBE/pyrobosim_flexbe_behaviors/CHANGELOG.rst new file mode 100644 index 0000000..113fa6a --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/CHANGELOG.rst @@ -0,0 +1,7 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package pyrobosim_flexbe_behaviors +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.0.1 (2023-07-21) +------------------ +* Initial release of ROS 2 example FlexBE behaviors diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/CMakeLists.txt b/technologies/FlexBE/pyrobosim_flexbe_behaviors/CMakeLists.txt new file mode 100644 index 0000000..d2412a5 --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.10) +project(pyrobosim_flexbe_behaviors) + +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) +find_package(rclpy REQUIRED) + +install(DIRECTORY + manifest + DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY + config + DESTINATION lib/${PROJECT_NAME} +) + +install(PROGRAMS + bin/copy_behavior + DESTINATION lib/${PROJECT_NAME} +) + +# Install Python modules +ament_python_install_package(${PROJECT_NAME}) + +ament_package() diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/bin/copy_behavior b/technologies/FlexBE/pyrobosim_flexbe_behaviors/bin/copy_behavior new file mode 100755 index 0000000..85da3e0 --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/bin/copy_behavior @@ -0,0 +1,100 @@ +#!/bin/bash + +# Copyright 2023 Christopher Newport University +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + + +# WARNING: Use at your own risk, this script does not protect against overwriting files, or mis-naming + +if [ $# -lt 1 ]; then + echo -e "\e[93mThis is an extremely simple script designed to copy behaviors\033[0m" + echo -e "\e[93mfrom the install folder to the src folder assumed to be a git repo\033[0m" + echo -e "\e[93mfor storage.\033[0m" + echo "" + echo -e "\e[93mIt requires a WORKSPACE_ROOT environment variable assuming a standard \033[0m" + echo -e "\e[93m \${WORKSPACE_ROOT}/install and \${WORKSPACE_ROOT}/src layout.\033[0m" + echo "" + echo -e "\e[93mRun this script from the base src folder for behaviors package\033[0m" + echo -e "\e[93m e.g., \${WORKSPACE_ROOT}/src/pyrobosim_flexbe\033[0m" + echo "" + echo -e "\e[93m Usage: ros2 run pyrobosim_flexbe_behaviors copy_behavior BEHAVIOR_FILE_BASE \033[0m" + echo -e "\e[93m where BEHAVIOR_FILE_BASE_NAME.xml is the saved manifest name\033[0m" + echo -e "\e[93m OPTIONAL_BEHAVIOR_PACKAGE defaults to 'pyrobosim_flexbe_behaviors'\033[0m" + echo "" + echo -e "\e[93m WARNING: Use at your own risk, this script does not protect against overwriting files or mis-naming\033[0m" + exit 2 +fi + + +beh=$(echo "$1" | cut -f 1 -d '.') # include only the base file name is someone pastes .xml name +pack="pyrobosim_flexbe_behaviors" # Default name uses this behaviors package +if [ $# -eq 2 ]; then + pack="$2" + echo "Using specified package '${pack}'!" +fi + +# A few basic checks before attempting to copy +if [ ! -d "${pack}" ]; then + echo "" + echo -e "\e[91m Package '${pack}' does not exist under current directory '${PWD}' !\033[0m" + echo "" + echo -e "\e[93mRun this script from the base src folder for behaviors package\033[0m" + echo -e "\e[93m e.g., \${WORKSPACE_ROOT}/src/pyrobosim_flexbe\033[0m" + exit +fi + +if [ ! -d "${pack}/${pack}" ]; then + echo "" + echo -e "\e[91m Behavior implementation folder '${pack}/${pack}' does not exist under current directory '${PWD}' !\033[0m" + echo "" + echo -e "\e[93mRun this script from the base src folder for behaviors package\033[0m" + echo -e "\e[93m e.g., \${WORKSPACE_ROOT}/src/pyrobosim_flexbe\033[0m" + exit +fi + +if [ ! -f "${WORKSPACE_ROOT}/install/${pack}/local/lib/python3.10/dist-packages/${pack}/${beh}_sm.py" ]; then + echo "" + echo -e "\e[91m Behavior '${beh}' implementation does not exist in install folder for '${pack}' package!\033[0m" + echo "" + echo "Available behavior manifests in '${pack}' :" + ls ${WORKSPACE_ROOT}/install/${pack}/lib/${pack}/manifest + echo "" + echo -e "\e[93mConfirm behavior and package names!\033[0m" + exit +fi + + +# Begin the work +echo "Copying '${beh}' implementation to '${pack}' under '${PWD}' ..." +cp ${WORKSPACE_ROOT}/install/${pack}/local/lib/python3.10/dist-packages/${pack}/${beh}_sm.py ./${pack}/${pack}/${beh}_sm.py + +if test $? -eq 0; then + echo "Copying '${beh}.xml' manifest to '${pack}' ..." + + cp ${WORKSPACE_ROOT}/install/${pack}/lib/${pack}/manifest/${beh}.xml ./${pack}/manifest/${beh}.xml + if test $? -eq 0; then + echo "" + echo -e "\e[92mDone copying behavior '${beh}' to '${pack}'!\033[0m" + echo "Do not forget to git commit and git push any changes." + exit + else + echo "" + echo -e "\e[91m Failed to copy behavior '${beh}.xml' manifest after copying behavior implementation!\033[0m" + exit + fi +else + echo "" + echo -e "\e[91m Failed to copy behavior '${beh}'!\033[0m" + exit +fi diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/config/example.yaml b/technologies/FlexBE/pyrobosim_flexbe_behaviors/config/example.yaml new file mode 100644 index 0000000..e3f24de --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/config/example.yaml @@ -0,0 +1 @@ +# You can use this folder to place general config files diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/delib_ws_p2.xml b/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/delib_ws_p2.xml new file mode 100644 index 0000000..1e0c067 --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/delib_ws_p2.xml @@ -0,0 +1,30 @@ + + + + + + pyrobosim, roscon24 + David Conner + Sat Aug 31 2024 + + Behavior demonstrating p2 - travel, open door, go to table, pick object, + transport, and place + + + + + + + + + + + + + + + + + + + diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/delib_ws_p2_sm.xml b/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/delib_ws_p2_sm.xml new file mode 100644 index 0000000..fdd1e66 --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/delib_ws_p2_sm.xml @@ -0,0 +1,28 @@ + + + + + + pyrobosim, roscon24 + David Conner + Sat Aug 31 2024 + + Behavior demonstrating p2 - travel, open door, go to table, pick object, + transport, and place + + + + + + + + + + + + + + + + + diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/detectselect.xml b/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/detectselect.xml new file mode 100644 index 0000000..56dc752 --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/detectselect.xml @@ -0,0 +1,27 @@ + + + + + + pyrobosim, plan + David Conner + Wed Aug 14 2024 + + Simple test of PlanPath action for pyrobosim with detection and selection + actions. + + + + + + + + + + + + + + + + diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/test_navigate.xml b/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/test_navigate.xml new file mode 100644 index 0000000..b2fd7ca --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/test_navigate.xml @@ -0,0 +1,18 @@ + + + + + + pyrobosim, navigate + David Conner + Thu Aug 08 2024 + + Test navigation state + + + + + + + + diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/test_pick_place.xml b/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/test_pick_place.xml new file mode 100644 index 0000000..c625d63 --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/test_pick_place.xml @@ -0,0 +1,28 @@ + + + + + + pyrobosim, navigate + David Conner + Wed Aug 14 2024 + + Test Navigation, Pick and Place states + + + + + + + + + + + + + + + + + + diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/test_plan_path.xml b/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/test_plan_path.xml new file mode 100644 index 0000000..52cb190 --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/test_plan_path.xml @@ -0,0 +1,18 @@ + + + + + + pyrobosim, plan + David Conner + Wed Aug 14 2024 + + Simple test of PlanPath action for pyrobosim + + + + + + + + diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/package.xml b/technologies/FlexBE/pyrobosim_flexbe_behaviors/package.xml new file mode 100644 index 0000000..a67c7e1 --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/package.xml @@ -0,0 +1,33 @@ + + + + pyrobosim_flexbe_behaviors + 0.0.1 + + All implemented FlexBE behaviors for ROSCon '24 + Deliberative Technologies Workshop. + + David Conner + David Conner + Apache 2 + + rclpy + flexbe_behavior_engine + flexbe_webui + pyrobosim_flexbe_states + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + ament_cmake + ament_cmake_python + + + + ament_cmake + + diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/__init__.py b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/delib_ws_p2_sm.py b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/delib_ws_p2_sm.py new file mode 100644 index 0000000..6047fbf --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/delib_ws_p2_sm.py @@ -0,0 +1,172 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +# Copyright 2024 David Conner +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +########################################################### +# WARNING: Generated code! # +# ************************** # +# Manual changes may get lost if file is generated again. # +# Only code inside the [MANUAL] tags will be kept. # +########################################################### + +""" +Define delib_ws_p2. + +Behavior demonstrating p2 - travel, open door, go to table, pick object, +transport, and place + +Created on Sat Aug 31 2024 +@author: David Conner +""" + + +from flexbe_core import Autonomy +from flexbe_core import Behavior +from flexbe_core import ConcurrencyContainer +from flexbe_core import Logger +from flexbe_core import OperatableStateMachine +from flexbe_core import PriorityContainer +from flexbe_core import initialize_flexbe_core +from flexbe_states.log_key_state import LogKeyState +from flexbe_states.operator_decision_state import OperatorDecisionState +from pyrobosim_flexbe_behaviors.detectselect_sm import DetectSelectSM +from pyrobosim_flexbe_states.door_action_state import DoorActionState +from pyrobosim_flexbe_states.follow_path_state import FollowPathState +from pyrobosim_flexbe_states.plan_path_state import PlanPathState + +# Additional imports can be added inside the following tags +# [MANUAL_IMPORT] + + +# [/MANUAL_IMPORT] + + +class delibwsp2SM(Behavior): + """ + Define delib_ws_p2. + + Behavior demonstrating p2 - travel, open door, go to table, pick object, + transport, and place + """ + + def __init__(self, node): + super().__init__() + self.name = 'delib_ws_p2' + + # parameters of this behavior + self.add_parameter('door_location', 'hall_dining_trash') + self.add_parameter('get_location', 'desk') + self.add_parameter('put_location', 'dumpster') + + # Initialize ROS node information + initialize_flexbe_core(node) + + # references to used behaviors + self.add_behavior(DetectSelectSM, 'DetectSelect', node) + + # Additional initialization code can be added inside the following tags + # [MANUAL_INIT] + + + # [/MANUAL_INIT] + + # Behavior comments: + + def create(self): + """Create state machine.""" + # Root state machine + # x:78 y:340, x:943 y:183 + _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) + _state_machine.userdata.goal = self.door_location + + # Additional creation code can be added inside the following tags + # [MANUAL_CREATE] + + + # [/MANUAL_CREATE] + + with _state_machine: + # x:131 y:80 + OperatableStateMachine.add('OpDecision', + OperatorDecisionState(outcomes=['open', 'go', 'quit'], + hint="Go to table", + suggestion='go'), + transitions={'open': 'PlanToDoor' # 292 105 -1 -1 -1 -1 + , 'go': 'DetectSelect' # 188 323 -1 -1 448 414 + , 'quit': 'finished' # 106 234 170 133 -1 -1 + }, + autonomy={'open': Autonomy.Full, + 'go': Autonomy.High, + 'quit': Autonomy.Full}) + + # x:449 y:388 + OperatableStateMachine.add('DetectSelect', + self.use_behavior(DetectSelectSM, 'DetectSelect', + parameters={'move_location': self.get_location, + 'place_location': self.put_location}), + transitions={'finished': 'finished' # 267 390 448 429 -1 -1 + , 'failed': 'OpDecision' # 232 304 -1 -1 197 133 + }, + autonomy={'finished': Autonomy.Full, 'failed': Autonomy.High}) + + # x:559 y:83 + OperatableStateMachine.add('GoToDoor', + FollowPathState(action_topic='robot/follow_path', + server_timeout=2.0), + transitions={'done': 'OpenDoor' # 754 117 -1 -1 786 183 + , 'failed': 'LogFailed' # 471 177 558 130 420 240 + }, + autonomy={'done': Autonomy.High, 'failed': Autonomy.Off}, + remapping={'path': 'path', 'msg': 'msg'}) + + # x:369 y:241 + OperatableStateMachine.add('LogFailed', + LogKeyState(text="Failed - {}", + severity=2), + transitions={'done': 'OpDecision' # 243 237 -1 -1 212 133 + }, + autonomy={'done': Autonomy.Off}, + remapping={'data': 'msg'}) + + # x:742 y:184 + OperatableStateMachine.add('OpenDoor', + DoorActionState(action_type='open', + robot_name='robot', + action_topic='/execute_action', + timeout=2.0), + transitions={'done': 'DetectSelect' # 581 304 741 223 540 387 + , 'failed': 'LogFailed' # 582 226 -1 -1 -1 -1 + }, + autonomy={'done': Autonomy.High, 'failed': Autonomy.Off}, + remapping={'msg': 'msg'}) + + # x:326 y:80 + OperatableStateMachine.add('PlanToDoor', + PlanPathState(action_topic='robot/plan_path', + timeout=10.0), + transitions={'done': 'GoToDoor' # 503 91 -1 -1 -1 -1 + , 'failed': 'LogFailed' # 390 200 -1 -1 -1 -1 + }, + autonomy={'done': Autonomy.High, 'failed': Autonomy.Off}, + remapping={'goal': 'goal', 'msg': 'msg', 'path': 'path'}) + + return _state_machine + + # Private functions can be added inside the following tags + # [MANUAL_FUNC] + + + # [/MANUAL_FUNC] diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/delib_ws_p2_sm_sm.py b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/delib_ws_p2_sm_sm.py new file mode 100644 index 0000000..e860ea4 --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/delib_ws_p2_sm_sm.py @@ -0,0 +1,246 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +# Copyright 2024 David Conner +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +########################################################### +# WARNING: Generated code! # +# ************************** # +# Manual changes may get lost if file is generated again. # +# Only code inside the [MANUAL] tags will be kept. # +########################################################### + +""" +Define delib_ws_p2_sm. + +Behavior demonstrating p2 - travel, open door, go to table, pick object, +transport, and place + +Created on Sat Aug 31 2024 +@author: David Conner +""" + + +from flexbe_core import Autonomy +from flexbe_core import Behavior +from flexbe_core import ConcurrencyContainer +from flexbe_core import Logger +from flexbe_core import OperatableStateMachine +from flexbe_core import PriorityContainer +from flexbe_core import initialize_flexbe_core +from flexbe_states.log_key_state import LogKeyState +from flexbe_states.operator_decision_state import OperatorDecisionState +from flexbe_states.user_data_state import UserdataState +from pyrobosim_flexbe_behaviors.detectselect_sm import DetectSelectSM +from pyrobosim_flexbe_states.door_action_state import DoorActionState +from pyrobosim_flexbe_states.follow_path_state import FollowPathState +from pyrobosim_flexbe_states.plan_path_state import PlanPathState + +# Additional imports can be added inside the following tags +# [MANUAL_IMPORT] + + +# [/MANUAL_IMPORT] + + +class delibwsp2smSM(Behavior): + """ + Define delib_ws_p2_sm. + + Behavior demonstrating p2 - travel, open door, go to table, pick object, + transport, and place + """ + + def __init__(self, node): + super().__init__() + self.name = 'delib_ws_p2_sm' + + # parameters of this behavior + self.add_parameter('get_location', 'desk') + self.add_parameter('put_location', 'dumpster') + + # Initialize ROS node information + initialize_flexbe_core(node) + + # references to used behaviors + self.add_behavior(DetectSelectSM, 'DetectSelect', node) + + # Additional initialization code can be added inside the following tags + # [MANUAL_INIT] + + + # [/MANUAL_INIT] + + # Behavior comments: + + def create(self): + """Create state machine.""" + # Root state machine + # x:78 y:340, x:684 y:262 + _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) + + # Additional creation code can be added inside the following tags + # [MANUAL_CREATE] + + + # [/MANUAL_CREATE] + + # x:1160 y:383, x:775 y:411 + _sm_opendoorsm_0 = OperatableStateMachine(outcomes=['finished', 'failed'], + output_keys=['msg']) + + with _sm_opendoorsm_0: + # x:97 y:32 + OperatableStateMachine.add('ChooseGoal', + OperatorDecisionState(outcomes=['dumpster', + 'hall_dining_trash', + 'hall_kitchen_office', + 'hall_kitchen_trash', + 'quit'], + hint=None, + suggestion=None), + transitions={'dumpster': 'Dumpster' # 321 52 -1 -1 -1 -1 + , 'hall_dining_trash': 'DiningTrashDoor' # 316 125 -1 -1 -1 -1 + , 'hall_kitchen_office': 'KitchenOfficeDoor' # 284 216 211 85 -1 -1 + , 'hall_kitchen_trash': 'KitchenTrash' # 262 287 159 85 -1 -1 + , 'quit': 'QuitMsg' # 254 392 127 85 -1 -1 + }, + autonomy={'dumpster': Autonomy.Off, + 'hall_dining_trash': Autonomy.Off, + 'hall_kitchen_office': Autonomy.Off, + 'hall_kitchen_trash': Autonomy.Off, + 'quit': Autonomy.Off}) + + # x:395 y:110 + OperatableStateMachine.add('DiningTrashDoor', + UserdataState(data='hall_dining_trash'), + transitions={'done': 'PlanToDoor' # 559 131 -1 -1 -1 -1 + }, + autonomy={'done': Autonomy.Off}, + remapping={'data': 'goal'}) + + # x:399 y:46 + OperatableStateMachine.add('Dumpster', + UserdataState(data='dumpster'), + transitions={'done': 'PlanToDoor' # 570 87 -1 -1 638 127 + }, + autonomy={'done': Autonomy.Off}, + remapping={'data': 'goal'}) + + # x:817 y:170 + OperatableStateMachine.add('GoToDoor', + FollowPathState(action_topic='robot/follow_path', + server_timeout=2.0), + transitions={'done': 'OpenDoor', + 'failed': 'failed' # 789 314 -1 -1 -1 -1 + }, + autonomy={'done': Autonomy.High, 'failed': Autonomy.Off}, + remapping={'path': 'path', 'msg': 'msg'}) + + # x:399 y:185 + OperatableStateMachine.add('KitchenOfficeDoor', + UserdataState(data='hall_kitchen_office'), + transitions={'done': 'PlanToDoor' # 569 173 -1 -1 -1 -1 + }, + autonomy={'done': Autonomy.Off}, + remapping={'data': 'goal'}) + + # x:399 y:262 + OperatableStateMachine.add('KitchenTrash', + UserdataState(data='hall_kitchen_trash'), + transitions={'done': 'PlanToDoor' # 567 230 -1 -1 625 181 + }, + autonomy={'done': Autonomy.Off}, + remapping={'data': 'goal'}) + + # x:1026 y:250 + OperatableStateMachine.add('OpenDoor', + DoorActionState(action_type='open', + robot_name='robot', + action_topic='/execute_action', + timeout=2.0), + transitions={'done': 'finished' # 1108 373 1118 303 -1 -1 + , 'failed': 'failed' # 989 374 -1 -1 811 418 + }, + autonomy={'done': Autonomy.High, 'failed': Autonomy.Off}, + remapping={'msg': 'msg'}) + + # x:596 y:128 + OperatableStateMachine.add('PlanToDoor', + PlanPathState(action_topic='robot/plan_path', + timeout=10.0), + transitions={'done': 'GoToDoor', + 'failed': 'failed' # 710 360 688 181 770 426 + }, + autonomy={'done': Autonomy.High, 'failed': Autonomy.Off}, + remapping={'goal': 'goal', 'msg': 'msg', 'path': 'path'}) + + # x:397 y:368 + OperatableStateMachine.add('QuitMsg', + UserdataState(data='User selected quit!'), + transitions={'done': 'failed' # 636 412 -1 -1 -1 -1 + }, + autonomy={'done': Autonomy.Off}, + remapping={'data': 'goal'}) + + with _state_machine: + # x:210 y:90 + OperatableStateMachine.add('OpDecision', + OperatorDecisionState(outcomes=['open', 'go', 'quit'], + hint="Go to table", + suggestion='go'), + transitions={'open': 'OpenDoorSM' # 411 88 -1 -1 -1 -1 + , 'go': 'DetectSelect' # 227 298 258 143 289 373 + , 'quit': 'finished' # 67 226 240 143 81 339 + }, + autonomy={'open': Autonomy.Full, + 'go': Autonomy.High, + 'quit': Autonomy.Full}) + + # x:290 y:347 + OperatableStateMachine.add('DetectSelect', + self.use_behavior(DetectSelectSM, 'DetectSelect', + parameters={'move_location': self.get_location, + 'place_location': self.put_location}), + transitions={'finished': 'finished' # 154 370 289 388 -1 -1 + , 'failed': 'OpDecision' # 282 282 337 346 285 143 + }, + autonomy={'finished': Autonomy.Full, 'failed': Autonomy.High}) + + # x:396 y:237 + OperatableStateMachine.add('LogFailed', + LogKeyState(text="failed - {}", + severity=2), + transitions={'done': 'OpDecision' # 327 229 -1 -1 310 143 + }, + autonomy={'done': Autonomy.Low}, + remapping={'data': 'msg'}) + + # x:471 y:106 + OperatableStateMachine.add('OpenDoorSM', + _sm_opendoorsm_0, + transitions={'finished': 'OpDecision' # 398 154 470 140 337 123 + , 'failed': 'LogFailed' # 516 233 519 165 -1 -1 + }, + autonomy={'finished': Autonomy.Low, 'failed': Autonomy.Off}, + remapping={'msg': 'msg'}) + + return _state_machine + + # Private functions can be added inside the following tags + # [MANUAL_FUNC] + + + # [/MANUAL_FUNC] diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/detectselect_sm.py b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/detectselect_sm.py new file mode 100644 index 0000000..2eedf2a --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/detectselect_sm.py @@ -0,0 +1,234 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +# Copyright 2024 David Conner +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +########################################################### +# WARNING: Generated code! # +# ************************** # +# Manual changes may get lost if file is generated again. # +# Only code inside the [MANUAL] tags will be kept. # +########################################################### + +""" +Define DetectSelect. + +Simple test of PlanPath action for pyrobosim with detection and selection +actions. + +Created on Wed Aug 14 2024 +@author: David Conner +""" + + +from flexbe_core import Autonomy +from flexbe_core import Behavior +from flexbe_core import ConcurrencyContainer +from flexbe_core import Logger +from flexbe_core import OperatableStateMachine +from flexbe_core import PriorityContainer +from flexbe_core import initialize_flexbe_core +from flexbe_states.log_key_state import LogKeyState +from flexbe_states.log_state import LogState +from flexbe_states.selection_state import SelectionState +from flexbe_states.user_data_state import UserdataState +from pyrobosim_flexbe_states.detect_objects_state import DetectObjectsState +from pyrobosim_flexbe_states.follow_path_state import FollowPathState +from pyrobosim_flexbe_states.pick_action_state import PickActionState +from pyrobosim_flexbe_states.place_action_state import PlaceActionState +from pyrobosim_flexbe_states.plan_path_state import PlanPathState + +# Additional imports can be added inside the following tags +# [MANUAL_IMPORT] + + +# [/MANUAL_IMPORT] + + +class DetectSelectSM(Behavior): + """ + Define DetectSelect. + + Simple test of PlanPath action for pyrobosim with detection and selection + actions. + """ + + def __init__(self, node): + super().__init__() + self.name = 'DetectSelect' + + # parameters of this behavior + self.add_parameter('move_location', 'counter0_right') + self.add_parameter('place_location', 'desk0') + + # Initialize ROS node information + initialize_flexbe_core(node) + + # references to used behaviors + + # Additional initialization code can be added inside the following tags + # [MANUAL_INIT] + + + # [/MANUAL_INIT] + + # Behavior comments: + + def create(self): + """Create state machine.""" + # Root state machine + # x:1283 y:68, x:673 y:457 + _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) + _state_machine.userdata.goal = self.move_location + + # Additional creation code can be added inside the following tags + # [MANUAL_CREATE] + + + # [/MANUAL_CREATE] + + with _state_machine: + # x:128 y:59 + OperatableStateMachine.add('PlanFirstMove', + PlanPathState(action_topic='robot/plan_path', + timeout=10.0), + transitions={'done': 'FollowFirstPath' # 354 58 -1 -1 -1 -1 + , 'failed': 'LogFailedMsg' # 204 408 262 112 325 484 + }, + autonomy={'done': Autonomy.High, 'failed': Autonomy.Off}, + remapping={'goal': 'goal', 'msg': 'msg', 'path': 'path'}) + + # x:912 y:30 + OperatableStateMachine.add('DetectObjects', + DetectObjectsState(filter=None, + action_topic='robot/detect_objects', + timeout=2.0), + transitions={'done': 'SelectObject' # 971 157 956 83 910 228 + , 'failed': 'LogFailedMsg' # 638 262 -1 -1 -1 -1 + , 'nothing': 'LogFailedMsg' # 638 262 -1 -1 -1 -1 + }, + autonomy={'done': Autonomy.High, + 'failed': Autonomy.Off, + 'nothing': Autonomy.Off}, + remapping={'goal': 'goal', 'msg': 'msg', 'items': 'items'}) + + # x:423 y:47 + OperatableStateMachine.add('FollowFirstPath', + FollowPathState(action_topic='robot/follow_path', + server_timeout=2.0), + transitions={'done': 'DetectObjects' # 763 54 -1 -1 -1 -1 + , 'failed': 'LogFailedMsg' # 363 361 447 100 358 444 + }, + autonomy={'done': Autonomy.High, 'failed': Autonomy.Off}, + remapping={'path': 'path', 'msg': 'msg'}) + + # x:794 y:615 + OperatableStateMachine.add('FollowNextPath', + FollowPathState(action_topic='robot/follow_path', + server_timeout=2.0), + transitions={'done': 'PlaceObject' # 1080 654 -1 -1 1131 603 + , 'failed': 'LogFailedMsg' # 580 580 -1 -1 -1 -1 + }, + autonomy={'done': Autonomy.High, 'failed': Autonomy.Off}, + remapping={'path': 'path', 'msg': 'msg'}) + + # x:326 y:445 + OperatableStateMachine.add('LogFailedMsg', + LogKeyState(text="Failed: {}", + severity=2), + transitions={'done': 'failed' # 537 469 -1 -1 -1 -1 + }, + autonomy={'done': Autonomy.Full}, + remapping={'data': 'msg'}) + + # x:615 y:321 + OperatableStateMachine.add('LogFailure', + LogState(text="Failed to select object", + severity=2), + transitions={'done': 'failed' # 542 416 -1 -1 -1 -1 + }, + autonomy={'done': Autonomy.High}) + + # x:1217 y:401 + OperatableStateMachine.add('LogFinished', + LogState(text="Placed object!", + severity=2), + transitions={'done': 'finished' # 1261 238 -1 -1 1288 99 + }, + autonomy={'done': Autonomy.Full}) + + # x:940 y:376 + OperatableStateMachine.add('NextLocation', + UserdataState(data=self.place_location), + transitions={'done': 'PlanNextMove' # 897 435 -1 -1 893 486 + }, + autonomy={'done': Autonomy.Off}, + remapping={'data': 'goal'}) + + # x:1030 y:310 + OperatableStateMachine.add('PickObject', + PickActionState(robot_name='robot', + action_topic='/execute_action', + timeout=2.0), + transitions={'done': 'NextLocation' # 1075 390 -1 -1 -1 -1 + , 'failed': 'LogFailedMsg' # 709 409 -1 -1 -1 -1 + }, + autonomy={'done': Autonomy.High, 'failed': Autonomy.Off}, + remapping={'object': 'object', 'msg': 'msg'}) + + # x:1079 y:550 + OperatableStateMachine.add('PlaceObject', + PlaceActionState(robot_name='robot', + action_topic='/execute_action', + timeout=2.0), + transitions={'done': 'LogFinished' # 1265 518 -1 -1 -1 -1 + , 'failed': 'LogFailedMsg' # 745 539 -1 -1 -1 -1 + }, + autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off}, + remapping={'msg': 'msg'}) + + # x:838 y:487 + OperatableStateMachine.add('PlanNextMove', + PlanPathState(action_topic='robot/plan_path', + timeout=2.0), + transitions={'done': 'FollowNextPath' # 844 575 870 540 831 614 + , 'failed': 'LogFailedMsg' # 602 490 -1 -1 -1 -1 + }, + autonomy={'done': Autonomy.High, 'failed': Autonomy.Off}, + remapping={'goal': 'goal', 'msg': 'msg', 'path': 'path'}) + + # x:869 y:229 + OperatableStateMachine.add('SelectObject', + SelectionState(message="Select item to pick", + timeout=1.0, + action_topic='flexbe/behavior_input'), + transitions={'received': 'PickObject' # 1041 257 -1 -1 1071 309 + , 'aborted': 'LogFailure' # 772 301 -1 -1 -1 -1 + , 'no_connection': 'LogFailure' # 772 301 -1 -1 -1 -1 + , 'data_error': 'LogFailure' # 772 301 -1 -1 -1 -1 + }, + autonomy={'received': Autonomy.High, + 'aborted': Autonomy.Low, + 'no_connection': Autonomy.Low, + 'data_error': Autonomy.Low}, + remapping={'items': 'items', 'data': 'object'}) + + return _state_machine + + # Private functions can be added inside the following tags + # [MANUAL_FUNC] + + + # [/MANUAL_FUNC] diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/test_navigate_sm.py b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/test_navigate_sm.py new file mode 100644 index 0000000..ca8d460 --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/test_navigate_sm.py @@ -0,0 +1,187 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +# Copyright 2024 David Conner +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +########################################################### +# WARNING: Generated code! # +# ************************** # +# Manual changes may get lost if file is generated again. # +# Only code inside the [MANUAL] tags will be kept. # +########################################################### + +""" +Define test navigate. + +Test navigation state + +Created on Thu Aug 08 2024 +@author: David Conner +""" + + +from flexbe_core import Autonomy +from flexbe_core import Behavior +from flexbe_core import ConcurrencyContainer +from flexbe_core import Logger +from flexbe_core import OperatableStateMachine +from flexbe_core import PriorityContainer +from flexbe_core import initialize_flexbe_core +from flexbe_states.log_state import LogState +from pyrobosim_flexbe_states.navigate_action_state import NavigateActionState + +# Additional imports can be added inside the following tags +# [MANUAL_IMPORT] + + +# [/MANUAL_IMPORT] + + +class testnavigateSM(Behavior): + """ + Define test navigate. + + Test navigation state + """ + + def __init__(self, node): + super().__init__() + self.name = 'test navigate' + + # parameters of this behavior + + # Initialize ROS node information + initialize_flexbe_core(node) + + # references to used behaviors + + # Additional initialization code can be added inside the following tags + # [MANUAL_INIT] + + + # [/MANUAL_INIT] + + # Behavior comments: + + def create(self): + """Create state machine.""" + # Private variables + action_topic = '/execute_action' + robot_name = 'robot' + + # Root state machine + # x:1252 y:96, x:1252 y:256 + _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) + + # Additional creation code can be added inside the following tags + # [MANUAL_CREATE] + + + # [/MANUAL_CREATE] + + with _state_machine: + # x:160 y:81 + OperatableStateMachine.add('Start', + LogState(text="Begin navigation ...", + severity=2), + transitions={'done': 'NavigateDesk' # 305 130 -1 -1 -1 -1 + }, + autonomy={'done': Autonomy.High}) + + # x:965 y:364 + OperatableStateMachine.add('LogCanceled', + LogState(text="Canceled navigation", + severity=2), + transitions={'done': 'failed' # 1149 321 -1 -1 -1 -1 + }, + autonomy={'done': Autonomy.Full}) + + # x:1086 y:90 + OperatableStateMachine.add('LogCounter', + LogState(text="Arrived at counter", + severity=2), + transitions={'done': 'finished' # 1221 89 -1 -1 -1 -1 + }, + autonomy={'done': Autonomy.High}) + + # x:629 y:102 + OperatableStateMachine.add('LogDesk', + LogState(text="Arrived at desk", + severity=2), + transitions={'done': 'NavigateCounter' # 753 104 -1 -1 -1 -1 + }, + autonomy={'done': Autonomy.High}) + + # x:1088 y:227 + OperatableStateMachine.add('LogFailed', + LogState(text="Arrived at desk", + severity=2), + transitions={'done': 'failed' # 1208 246 -1 -1 -1 -1 + }, + autonomy={'done': Autonomy.Full}) + + # x:908 y:498 + OperatableStateMachine.add('LogTimeout', + LogState(text="time out error", + severity=2), + transitions={'done': 'failed' # 1125 394 -1 -1 -1 -1 + }, + autonomy={'done': Autonomy.Full}) + + # x:806 y:103 + OperatableStateMachine.add('NavigateCounter', + NavigateActionState(target_location='counter', + robot_name=robot_name, + action_topic=action_topic, + server_timeout=5.0, + navigate_timeout=None), + transitions={'done': 'LogCounter' # 1053 96 -1 -1 -1 -1 + , 'planning_failed': 'LogFailed' # 1055 198 -1 -1 -1 -1 + , 'motion_failed': 'LogFailed' # 1055 198 -1 -1 -1 -1 + , 'canceled': 'LogCanceled' # 929 275 -1 -1 -1 -1 + , 'timeout': 'LogTimeout' # 850 448 853 156 -1 -1 + }, + autonomy={'done': Autonomy.Off, + 'planning_failed': Autonomy.Off, + 'motion_failed': Autonomy.Off, + 'canceled': Autonomy.Off, + 'timeout': Autonomy.Off}) + + # x:345 y:144 + OperatableStateMachine.add('NavigateDesk', + NavigateActionState(target_location='desk', + robot_name=robot_name, + action_topic=action_topic, + server_timeout=5.0, + navigate_timeout=None), + transitions={'done': 'LogDesk' # 575 134 -1 -1 -1 -1 + , 'planning_failed': 'LogFailed' # 814 255 -1 -1 -1 -1 + , 'motion_failed': 'LogFailed' # 814 255 -1 -1 -1 -1 + , 'canceled': 'LogCanceled' # 761 283 -1 -1 -1 -1 + , 'timeout': 'LogTimeout' # 718 478 -1 -1 907 540 + }, + autonomy={'done': Autonomy.Off, + 'planning_failed': Autonomy.Off, + 'motion_failed': Autonomy.Off, + 'canceled': Autonomy.Off, + 'timeout': Autonomy.Off}) + + return _state_machine + + # Private functions can be added inside the following tags + # [MANUAL_FUNC] + + + # [/MANUAL_FUNC] diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/test_pick_place_sm.py b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/test_pick_place_sm.py new file mode 100644 index 0000000..a05a258 --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/test_pick_place_sm.py @@ -0,0 +1,241 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +# Copyright 2024 David Conner +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +########################################################### +# WARNING: Generated code! # +# ************************** # +# Manual changes may get lost if file is generated again. # +# Only code inside the [MANUAL] tags will be kept. # +########################################################### + +""" +Define test pick place. + +Test Navigation, Pick and Place states + +Created on Wed Aug 14 2024 +@author: David Conner +""" + + +from flexbe_core import Autonomy +from flexbe_core import Behavior +from flexbe_core import ConcurrencyContainer +from flexbe_core import Logger +from flexbe_core import OperatableStateMachine +from flexbe_core import PriorityContainer +from flexbe_core import initialize_flexbe_core +from flexbe_states.log_state import LogState +from pyrobosim_flexbe_states.navigate_action_state import NavigateActionState +from pyrobosim_flexbe_states.pick_action_state import PickActionState +from pyrobosim_flexbe_states.place_action_state import PlaceActionState + +# Additional imports can be added inside the following tags +# [MANUAL_IMPORT] + + +# [/MANUAL_IMPORT] + + +class testpickplaceSM(Behavior): + """ + Define test pick place. + + Test Navigation, Pick and Place states + """ + + def __init__(self, node): + super().__init__() + self.name = 'test pick place' + + # parameters of this behavior + self.add_parameter('location', 'counter0_left') + self.add_parameter('object', 'banana') + self.add_parameter('return_location', 'desk0') + + # Initialize ROS node information + initialize_flexbe_core(node) + + # references to used behaviors + + # Additional initialization code can be added inside the following tags + # [MANUAL_INIT] + + + + + + + + + # [/MANUAL_INIT] + + # Behavior comments: + + def create(self): + """Create state machine.""" + # Private variables + action_topic = '/execute_action' + robot_name = 'robot' + + # Root state machine + # x:1283 y:96, x:1043 y:255 + _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) + _state_machine.userdata.object = self.object + + # Additional creation code can be added inside the following tags + # [MANUAL_CREATE] + + + + + + + + + # [/MANUAL_CREATE] + + with _state_machine: + # x:54 y:52 + OperatableStateMachine.add('Start', + LogState(text="Begin navigation ...", + severity=2), + transitions={'done': 'NavigateLocation' # 83 181 -1 -1 -1 -1 + }, + autonomy={'done': Autonomy.High}) + + # x:366 y:56 + OperatableStateMachine.add('LogArrived', + LogState(text="Arrived at ", + severity=2), + transitions={'done': 'PickObject' # 485 48 -1 -1 -1 -1 + }, + autonomy={'done': Autonomy.High}) + + # x:708 y:366 + OperatableStateMachine.add('LogCanceled', + LogState(text="Canceled navigation", + severity=2), + transitions={'done': 'failed' # 927 322 -1 -1 -1 -1 + }, + autonomy={'done': Autonomy.Full}) + + # x:734 y:212 + OperatableStateMachine.add('LogFailed', + LogState(text="Failed to complete task!", + severity=2), + transitions={'done': 'failed' # 960 240 -1 -1 -1 -1 + }, + autonomy={'done': Autonomy.Full}) + + # x:1200 y:206 + OperatableStateMachine.add('LogPlaced', + LogState(text="Placed object", + severity=2), + transitions={'done': 'finished' # 1257 157 -1 -1 1273 107 + }, + autonomy={'done': Autonomy.Off}) + + # x:1071 y:82 + OperatableStateMachine.add('LogReturn', + LogState(text="Arrived at ", + severity=2), + transitions={'done': 'PlaceObject' # 1130 272 -1 -1 1162 388 + }, + autonomy={'done': Autonomy.High}) + + # x:908 y:498 + OperatableStateMachine.add('LogTimeout', + LogState(text="time out error", + severity=2), + transitions={'done': 'failed' # 993 394 -1 -1 -1 -1 + }, + autonomy={'done': Autonomy.Full}) + + # x:121 y:182 + OperatableStateMachine.add('NavigateLocation', + NavigateActionState(target_location=self.location, + robot_name=robot_name, + action_topic=action_topic, + server_timeout=5.0, + navigate_timeout=None), + transitions={'done': 'LogArrived' # 307 101 258 190 -1 -1 + , 'planning_failed': 'LogFailed' # 502 242 -1 -1 -1 -1 + , 'motion_failed': 'LogFailed' # 502 242 -1 -1 -1 -1 + , 'canceled': 'LogCanceled' # 515 298 -1 -1 -1 -1 + , 'timeout': 'LogTimeout' # 642 480 -1 -1 907 540 + }, + autonomy={'done': Autonomy.Off, + 'planning_failed': Autonomy.Off, + 'motion_failed': Autonomy.Off, + 'canceled': Autonomy.Off, + 'timeout': Autonomy.Off}) + + # x:827 y:52 + OperatableStateMachine.add('NavigateReturn', + NavigateActionState(target_location=self.return_location, + robot_name=robot_name, + action_topic=action_topic, + server_timeout=5.0, + navigate_timeout=None), + transitions={'done': 'LogReturn' # 1042 68 -1 -1 -1 -1 + , 'planning_failed': 'LogFailed' # 745 176 -1 -1 -1 -1 + , 'motion_failed': 'LogFailed' # 745 176 -1 -1 -1 -1 + , 'canceled': 'LogCanceled' # 831 296 848 105 -1 -1 + , 'timeout': 'LogTimeout' # 862 442 874 105 -1 -1 + }, + autonomy={'done': Autonomy.Low, + 'planning_failed': Autonomy.Off, + 'motion_failed': Autonomy.Off, + 'canceled': Autonomy.Off, + 'timeout': Autonomy.Off}) + + # x:536 y:47 + OperatableStateMachine.add('PickObject', + PickActionState(robot_name='robot', + action_topic='/execute_action', + timeout=2.0), + transitions={'done': 'NavigateReturn' # 772 57 -1 -1 -1 -1 + , 'failed': 'LogFailed' # 596 185 634 100 -1 -1 + }, + autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off}, + remapping={'object': 'object', 'msg': 'msg'}) + + # x:1107 y:389 + OperatableStateMachine.add('PlaceObject', + PlaceActionState(robot_name='robot', + action_topic='/execute_action', + timeout=2.0), + transitions={'done': 'LogPlaced' # 1238 310 -1 -1 -1 -1 + , 'failed': 'LogFailed' # 947 287 -1 -1 -1 -1 + }, + autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off}, + remapping={'msg': 'msg'}) + + return _state_machine + + # Private functions can be added inside the following tags + # [MANUAL_FUNC] + + + + + + + + + # [/MANUAL_FUNC] diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/test_plan_path_sm.py b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/test_plan_path_sm.py new file mode 100644 index 0000000..efaa32e --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/test_plan_path_sm.py @@ -0,0 +1,144 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +# Copyright 2024 David Conner +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +########################################################### +# WARNING: Generated code! # +# ************************** # +# Manual changes may get lost if file is generated again. # +# Only code inside the [MANUAL] tags will be kept. # +########################################################### + +""" +Define Test Plan Path. + +Simple test of PlanPath action for pyrobosim + +Created on Wed Aug 14 2024 +@author: David Conner +""" + + +from flexbe_core import Autonomy +from flexbe_core import Behavior +from flexbe_core import ConcurrencyContainer +from flexbe_core import Logger +from flexbe_core import OperatableStateMachine +from flexbe_core import PriorityContainer +from flexbe_core import initialize_flexbe_core +from flexbe_states.log_key_state import LogKeyState +from pyrobosim_flexbe_states.follow_path_state import FollowPathState +from pyrobosim_flexbe_states.plan_path_state import PlanPathState + +# Additional imports can be added inside the following tags +# [MANUAL_IMPORT] + + +# [/MANUAL_IMPORT] + + +class TestPlanPathSM(Behavior): + """ + Define Test Plan Path. + + Simple test of PlanPath action for pyrobosim + """ + + def __init__(self, node): + super().__init__() + self.name = 'Test Plan Path' + + # parameters of this behavior + + # Initialize ROS node information + initialize_flexbe_core(node) + + # references to used behaviors + + # Additional initialization code can be added inside the following tags + # [MANUAL_INIT] + + + + + + + # [/MANUAL_INIT] + + # Behavior comments: + + def create(self): + """Create state machine.""" + # Root state machine + # x:1232 y:54, x:1214 y:117 + _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) + _state_machine.userdata.destination = 'table_source' + _state_machine.userdata.table = 'table_sink' + _state_machine.userdata.desk = 'desk0' + + # Additional creation code can be added inside the following tags + # [MANUAL_CREATE] + + + + + + + # [/MANUAL_CREATE] + + with _state_machine: + # x:128 y:59 + OperatableStateMachine.add('PlanCounter', + PlanPathState(action_topic='robot/plan_path', + timeout=2.0), + transitions={'done': 'FollowPath' # 428 72 -1 -1 -1 -1 + , 'failed': 'LogFailedMsg' # 488 225 262 112 -1 -1 + }, + autonomy={'done': Autonomy.High, 'failed': Autonomy.Off}, + remapping={'goal': 'destination', + 'msg': 'msg', + 'path': 'path'}) + + # x:501 y:44 + OperatableStateMachine.add('FollowPath', + FollowPathState(action_topic='robot/follow_path', + server_timeout=2.0), + transitions={'done': 'finished' # 953 64 -1 -1 -1 -1 + , 'failed': 'LogFailedMsg' # 684 211 611 97 -1 -1 + }, + autonomy={'done': Autonomy.High, 'failed': Autonomy.Off}, + remapping={'path': 'path', 'msg': 'msg'}) + + # x:670 y:321 + OperatableStateMachine.add('LogFailedMsg', + LogKeyState(text="Failed: {}", + severity=2), + transitions={'done': 'failed' # 991 234 -1 -1 -1 -1 + }, + autonomy={'done': Autonomy.Off}, + remapping={'data': 'msg'}) + + return _state_machine + + # Private functions can be added inside the following tags + # [MANUAL_FUNC] + + + + + + + # [/MANUAL_FUNC] diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/resource/pyrobosim_flexbe_behaviors b/technologies/FlexBE/pyrobosim_flexbe_behaviors/resource/pyrobosim_flexbe_behaviors new file mode 100644 index 0000000..e69de29 diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/setup.cfg b/technologies/FlexBE/pyrobosim_flexbe_behaviors/setup.cfg new file mode 100644 index 0000000..2cbb5c9 --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/pyrobosim_flexbe_behaviors +[install] +install_scripts=$base/lib/pyrobosim_flexbe_behaviors diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/setup.py b/technologies/FlexBE/pyrobosim_flexbe_behaviors/setup.py new file mode 100644 index 0000000..db0ead5 --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/setup.py @@ -0,0 +1,27 @@ +#!/usr/bin/env python +from setuptools import setup + +package_name = 'pyrobosim_flexbe_behaviors' + +setup( + name=package_name, + version='0.0.1', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='phil', + maintainer_email='philsplus@gmail.com', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'example_behavior_sm = pyrobosim_flexbe_behaviors.example_behavior_sm', + ], + }, +) diff --git a/technologies/FlexBE/pyrobosim_flexbe_states/CHANGELOG.rst b/technologies/FlexBE/pyrobosim_flexbe_states/CHANGELOG.rst new file mode 100644 index 0000000..5b5ba99 --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_states/CHANGELOG.rst @@ -0,0 +1,7 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package pyrobosim_flexbe_states +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.0.1 (2023-07-21) +------------------ +* Initial release of ROS 2 example FlexBE states diff --git a/technologies/FlexBE/pyrobosim_flexbe_states/package.xml b/technologies/FlexBE/pyrobosim_flexbe_states/package.xml new file mode 100644 index 0000000..d54231c --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_states/package.xml @@ -0,0 +1,35 @@ + + + + pyrobosim_flexbe_states + 0.0.1 + + All implemented FlexBE state implementations for pyrobosim interfaces. + + Implemented for ROSCon '24 Deliberative Technologies Workshop. + + Feel free to add new states. + + David Conner + David Conner + Apache 2 + + rclpy + flexbe_core + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + ament_cmake + ament_cmake_python + + + + ament_python + + + diff --git a/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/__init__.py b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/detect_local_objects_state.py b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/detect_local_objects_state.py new file mode 100644 index 0000000..bb033d9 --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/detect_local_objects_state.py @@ -0,0 +1,241 @@ +#!/usr/bin/env python + +# Copyright 2024 Christopher Newport University +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""FlexBE State to command PyRoboSim robot to plan a path to target location.""" + +from action_msgs.msg import GoalStatus + +from flexbe_core import EventState, Logger +from flexbe_core.proxy import ProxyActionClient, ProxyServiceCaller + +from pyrobosim_msgs.action import ExecuteTaskAction +from pyrobosim_msgs.srv import RequestWorldState +from pyrobosim_msgs.msg import ExecutionResult, TaskAction + +from rclpy.duration import Duration + + +class DetectLocalObjectsState(EventState): + """ + FlexBE state to request local objects for PyRoboSim robot + + Elements defined here for UI + Parameters + -- filter Item type to look for (default=None (detect all)) + -- action_topic Action topic name (default= '/execute_action') + -- server_topic Server topic to request observation (default='/request_world_state') + -- robot_name Name of robot (default='robot') + -- server_timeout Wait for action server timeout in seconds (default = 2s) + -- call_timeout Wait for service response to request (default=1s) + + Outputs + <= done Successfully retrieved items + <= failed Failed to plan path + + User data + #> msg string Result message + #> items list Detected items + """ + + def __init__(self, filter=None, + action_topic='/execute_action', + server_topic='/request_world_state', + robot_name='robot', + server_timeout=1.0, + call_timeout=1.0): + # See example_state.py for basic explanations. + super().__init__(outcomes=['done', 'failed'], + input_keys=['goal'], + output_keys=['msg', 'items']) + + self._goal = None + + self._filter = filter + self._action_topic = action_topic + self._srv_topic = server_topic + self._robot_name = robot_name + + self._server_timeout_sec = server_timeout + self._call_timeout = Duration(seconds=call_timeout) + + self._srv_client = None + self._act_client = None + self._srv_start_time = None # Reset timer for call timeout + self._return = None # Retain return value in case the outcome is blocked by operator + self._service_called = False + + def on_start(self): + # Set up the proxy now, but do not wait on the service just yet + self._srv_client = ProxyServiceCaller({self._srv_topic: RequestWorldState}, wait_duration=0.0) + self._act_client = ProxyActionClient({self._action_topic: ExecuteTaskAction}, + wait_duration=0.0) # no need to wait here, we'll check on_enter + + def on_stop(self): + # Remove the proxy client if no longer in use + ProxyServiceCaller.remove_client(self._srv_topic) + ProxyActionClient.remove_client(self._action_topic) + self._srv_client = None + self._act_client = None + + def execute(self, userdata): + """ + Call periodically while this state is active. + + Check if the action has been finished and evaluate the result. + """ + if self._return is not None: + # Return prior outcome in case transition is blocked by autonomy level + return self._return + + if self._service_called: + # Waiting for result from world state + # We will do this in a non-blocking way + if self._srv_client.done(self._srv_topic): + result = self._srv_client.result(self._srv_topic) # grab empty result, but nothing to check here presume success + Logger.localinfo(f"Got {type(result)} from service call to '{self._srv_topic} ' ...") + userdata.items = self._process_srv_response(result) + Logger.localinfo(f" returned '{self._return}' with {len(userdata.items) if userdata.items else 0}") + elif self._node.get_clock().now().nanoseconds - self._srv_start_time.nanoseconds > self._call_timeout.nanoseconds: + # Failed to return call in timely manner + self._return = 'failed' + Logger.logerr(f"{self._name}: Service {self._srv_topic} call timed out!") + return self._return + + # We have not finished detection yet + try: + status = self._act_client.get_status(self._action_topic) # get status before clearing result + if self._act_client.has_result(self._action_topic): + # Check if the action has been finished + result = self._act_client.get_result(self._action_topic, clear=True) + Logger.localinfo(f" '{self}' : '{self._action_topic}' returned {status} {result}") + if status == GoalStatus.STATUS_SUCCEEDED: + result_status = result.execution_result.status + userdata.items = None + if result_status == ExecutionResult.SUCCESS: + if self._srv_client.is_available(self._srv_topic, wait_duration=0.0): + # Non-blocking check for availability + Logger.localinfo(f"{self} - detection completed - now request items from service result") + self._do_service_call() + elif self._srv_start_time is None: + Logger.localinfo(f"{self} - detection completed - wait for world state server to become available") + self._srv_start_time = self._node.get_clock().now() # Reset timer for call timeout + elif self._node.get_clock().now().nanoseconds - self._srv_start_time.nanoseconds > self._call_timeout.nanoseconds: + # Failed to return call in timely manner + Logger.logerr(f"{self._name}: Service {self._srv_topic} is not available!") + self._return = 'failed' + # Otherwise waiting for service to become available + + # Otherwise check for action status change + if status == GoalStatus.STATUS_CANCELED: + Logger.loginfo(f" '{self}' : '{self._action_topic}' - detection goal was canceled! ") + self._return = 'failed' + elif status == GoalStatus.STATUS_ABORTED: + Logger.loginfo(f" '{self}' : '{self._action_topic}' - detection goal was aborted! ") + self._return = 'failed' + except Exception as exc: # pylint: disable=W0703 + Logger.logerr(f"{self._name}: {self._action_topic} exception {type(exc)} - {str(exc)}") + self._return = 'failed' + + # If the action has not yet finished, None outcome will be returned and the state stays active. + return self._return + + def _do_service_call(self): + """Make the service call using async non-blocking.""" + try: + Logger.localinfo(f"{self._name}: Calling service {self._srv_topic} ...") + srv_request = RequestWorldState.Request(robot=self._robot_name) + self._srv_client.call_async(self._srv_topic, srv_request, wait_duration=self._call_timeout.nanoseconds*1e-9) + self._service_called = True + except Exception as exc: + Logger.logerr(f"{self._name}: Service {self._srv_topic} exception {type(exc)} - {str(exc)}") + raise exc + + def _process_srv_response(self, response): + + robot = None + for rbot in response.state.robots: + if rbot.name == self._robot_name: + robot = rbot + break + + if robot is None: + Logger.logerr(f"Robot '{self._robot_name}' was not found in world state!") + self._return = 'failed' + return None + + Logger.localinfo(f"Robot '{self._robot_name}' at location '{robot.last_visited_location}'") + + local_objects = [] + for obj in response.state.objects: + if obj.parent == robot.last_visited_location: + if self._filter is None: + local_objects.append(obj.name) + elif obj.name.startswith(self._filter): + local_objects.append(obj.name) + self._return = 'done' + return tuple(local_objects) + + def on_enter(self, userdata): + """Call when state becomes active.""" + # make sure to reset the data from prior executions + self._srv_start_time = None # Reset timer for call timeout + self._return = None + self._service_called = False + userdata.items = None + self._act_client.remove_result(self._action_topic) # clear any prior result from action server + + # Send the goal. + try: + Logger.localinfo(f"Send detection request for '{self._robot_name}' using '{self._filter}' ") + goal = ExecuteTaskAction.Goal() + if self._filter is None: + goal.action = TaskAction(robot=self._robot_name, + type="detect", + ) + else: + goal.action = TaskAction(robot=self._robot_name, + type="detect", + object=self._filter + ) + self._act_client.send_goal(self._action_topic, goal, wait_duration=self._server_timeout_sec) + except Exception as exc: # pylint: disable=W0703 + # Since a state failure not necessarily causes a behavior failure, + # it is recommended to only print warnings, not errors. + # Using a linebreak before appending the error log enables the operator to collapse details in the GUI. + Logger.logwarn(f"Failed to send the '{self}' command:\n {type(exc)} - {exc}") + self._return = 'failed' + + def on_exit(self, userdata): + """Call when state is deactivated.""" + # Make sure that the action is not running when leaving this state. + # A situation where the action would still be active is for example when the operator manually triggers an outcome. + + if self._act_client.is_active(self._action_topic): + self._act_client.cancel(self._action_topic) + # Check for action status change + status = self._act_client.get_status(self._action_topic) # get status before clearing result + if status == GoalStatus.STATUS_CANCELED: + Logger.loginfo(f" '{self}' : '{self._action_topic}' - request to plan was canceled! ") + self._return = 'failed' + else: + status_string = self._act_client.get_status_string(self._action_topic) + Logger.loginfo(f" '{self}' : '{self._action_topic}' - Requested to cancel an active plan request ({status_string}).") + + # Local message are shown in terminal but not the UI + if self._return == 'done': + Logger.localinfo(f'Successfully retrieved detected items.') + else: + Logger.localwarn('Failed to detect items.') diff --git a/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/detect_objects_state.py b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/detect_objects_state.py new file mode 100644 index 0000000..b63d69c --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/detect_objects_state.py @@ -0,0 +1,184 @@ +#!/usr/bin/env python + +# Copyright 2024 Christopher Newport University +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""FlexBE State to command PyRoboSim robot to detect objects at the current location.""" + +from action_msgs.msg import GoalStatus + +from flexbe_core import EventState, Logger +from flexbe_core.proxy import ProxyActionClient + +from pyrobosim_msgs.action import DetectObjects +from pyrobosim_msgs.msg import ExecutionResult, TaskAction + +from rclpy.duration import Duration + + +class DetectObjectsState(EventState): + """ + FlexBE state to request local objects for PyRoboSim robot + + Elements defined here for UI + Parameters + -- filter Item type to look for (default=None (detect all)) + -- action_topic Action topic name (default= 'robot/detect_objects') + -- timeout Total time for action server to complete in seconds (default = 2s) + + Outputs + <= done Successfully retrieved 1 or more items + <= nothing No objects were detected + <= failed Action failed + + User data + #> msg string Result message + #> items list Detected items + """ + + def __init__(self, filter=None, + action_topic='robot/detect_objects', + timeout=1.0): + # See example_state.py for basic explanations. + super().__init__(outcomes=['done', 'failed', 'nothing'], + input_keys=['goal'], + output_keys=['msg', 'items']) + + self._goal = None + + self._filter = filter + self._action_topic = action_topic + + self._timeout = Duration(seconds=timeout) + + self._act_client = None + self._start_time = None # Reset timer for call timeout + self._return = None # Retain return value in case the outcome is blocked by operator + self._service_called = False + + def on_start(self): + # Set up the proxy now, but do not wait on the service just yet + self._act_client = ProxyActionClient({self._action_topic: DetectObjects}, + wait_duration=0.0) # no need to wait here, we'll check on_enter + + def on_stop(self): + # Remove the proxy client if no longer in use + ProxyActionClient.remove_client(self._action_topic) + self._act_client = None + + def execute(self, userdata): + """ + Call periodically while this state is active. + + Check if the action has been finished and evaluate the result. + """ + if self._return is not None: + # Return prior outcome in case transition is blocked by autonomy level + return self._return + + # We have not finished detection yet + try: + status = self._act_client.get_status(self._action_topic) # get status before clearing result + if self._act_client.has_result(self._action_topic): + # Check if the action has been finished + result = self._act_client.get_result(self._action_topic, clear=True) + if status == GoalStatus.STATUS_SUCCEEDED: + result_status = result.execution_result.status + userdata.msg = result.execution_result.message + if result_status == ExecutionResult.SUCCESS: + Logger.localinfo(f" '{self}' : '{self._action_topic}' " + f"detected {len(result.detected_objects)} objects") + if len(result.detected_objects)==0: + userdata.items = None + userdata.msg = f"{self.name}: found no objects at location!" + self._return = 'nothing' + else: + objects = result.detected_objects + names = [] + for obj in objects: + names.append(obj.name) + + userdata.items = names + self._return = 'done' + else: + Logger.localwarn(f"{self} : '{self._action_topic}' -" + f" failed to detect objects ({result_status}) '{userdata.msg}'") + userdata.items = None + self._return = 'failed' + elif self._node.get_clock().now().nanoseconds - self._start_time.nanoseconds > self._timeout.nanoseconds: + # Failed to return call in timely manner + self._return = 'failed' + userdata.msg = f"{self._name}: failed to get objects within timeout!" + Logger.localwarn(userdata.msg) + + # Otherwise check for action status change + if status == GoalStatus.STATUS_CANCELED: + Logger.loginfo(f" '{self}' : '{self._action_topic}' - detection goal was canceled! ") + self._return = 'failed' + elif status == GoalStatus.STATUS_ABORTED: + Logger.loginfo(f" '{self}' : '{self._action_topic}' - detection goal was aborted! ") + self._return = 'failed' + except Exception as exc: # pylint: disable=W0703 + Logger.logerr(f"{self._name}: {self._action_topic} exception {type(exc)} - {str(exc)}") + self._return = 'failed' + + # If the action has not yet finished, None outcome will be returned and the state stays active. + return self._return + + def on_enter(self, userdata): + """Call when state becomes active.""" + # make sure to reset the data from prior executions + self._return = None + userdata.items = None + self._act_client.remove_result(self._action_topic) # clear any prior result from action server + self._start_time = self._node.get_clock().now() + + # Send the goal. + try: + Logger.localinfo(f"Send detection request to '{self._action_topic}' using '{self._filter}' ") + if self._filter is None: + goal = DetectObjects.Goal() + else: + goal = DetectObjects.Goal(target_object=self._filter) + self._act_client.send_goal(self._action_topic, goal, wait_duration=self._timeout.nanoseconds*1e-9) + except Exception as exc: # pylint: disable=W0703 + # Since a state failure not necessarily causes a behavior failure, + # it is recommended to only print warnings, not errors. + # Using a linebreak before appending the error log enables the operator to collapse details in the GUI. + Logger.logwarn(f"Failed to send the '{self}' command:\n {type(exc)} - {exc}") + self._return = 'failed' + + def on_exit(self, userdata): + """Call when state is deactivated.""" + # Make sure that the action is not running when leaving this state. + # A situation where the action would still be active is for example when the operator manually triggers an outcome. + + if self._act_client.is_active(self._action_topic): + self._act_client.cancel(self._action_topic) + # Check for action status change + status = self._act_client.get_status(self._action_topic) # get status before clearing result + if status == GoalStatus.STATUS_CANCELED: + # log messages are shown on the UI and terminal (keep to minimum for normal operation) + Logger.loginfo(f" '{self}' : '{self._action_topic}' - request for objects was canceled! ") + self._return = 'failed' + else: + status_string = self._act_client.get_status_string(self._action_topic) + Logger.loginfo(f" '{self}' : '{self._action_topic}' - Requested to cancel an active detection request ({status_string}).") + + # Local message are shown in terminal but not the UI + # Generally avoid doing this except when debugging + if self._return == 'done': + Logger.localinfo(f'Successfully retrieved detected items.') + else: + Logger.localwarn('Failed to detect items.') diff --git a/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/door_action_state.py b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/door_action_state.py new file mode 100644 index 0000000..0fb99df --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/door_action_state.py @@ -0,0 +1,173 @@ +#!/usr/bin/env python + +# Copyright 2024 Christopher Newport University +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""FlexBE State to command PyRoboSim robot to open or close a door at current location.""" + +from action_msgs.msg import GoalStatus + +from flexbe_core import EventState, Logger +from flexbe_core.core import StateError +from flexbe_core.proxy import ProxyActionClient + +from pyrobosim_msgs.action import ExecuteTaskAction +from pyrobosim_msgs.msg import ExecutionResult, TaskAction + +from rclpy.duration import Duration + + +class DoorActionState(EventState): + """ + FlexBE state to perform open/close action for PyRoboSim robot at current location. + + Elements defined here for UI + Parameters + -- action_type Action either 'open' or 'close' + -- robot_name Robot name (default= 'robot') + -- action_topic Action topic name (default= '/execute_action') + -- timeout Total time to wait for action to complete in seconds (default = 2s) + + Outputs + <= done Successful completion + <= failed Failed to perform action + + User data + #> msg string Result message + """ + + def __init__(self, + action_type='open', + robot_name='robot', + action_topic='/execute_action', + timeout=2.0): + # See example_state.py for basic explanations. + super().__init__(outcomes=['done', 'failed'], + input_keys=[], + output_keys=['msg']) + + if action_type not in ('open', 'close'): + # Fail on behavior build if not valid action + msg = f"State '{self}' : Invalid action type '{action_type}' - must be 'open' or 'close'" + Logger.logerr(msg) + raise StateError(msg) + + self._action_type = action_type + self._action_outcome = {'open': 'opened', 'close': 'closed'}[action_type] # grammar + + self._goal = ExecuteTaskAction.Goal() + self._goal.action = TaskAction(robot=robot_name, type=action_type) + + self._topic = action_topic + self._timeout = Duration(seconds=timeout) + + self._client = ProxyActionClient({self._topic: ExecuteTaskAction}, + wait_duration=0.0) # pass required clients as dict (topic: type) + + self._start_time = None + self._return = None # Retain return value in case the outcome is blocked by operator + + def execute(self, userdata): + """ + Call periodically while this state is active. + + Check if the action has been finished and evaluate the result. + """ + if self._return is not None: + # Return prior outcome in case transition is blocked by autonomy level + return self._return + + status = self._client.get_status(self._topic) # get status before clearing result + if self._client.has_result(self._topic): + # Check if the action has been finished + result = self._client.get_result(self._topic, clear=True).execution_result + userdata.msg = result.message # Output message + if status == GoalStatus.STATUS_SUCCEEDED: + if result.status == ExecutionResult.SUCCESS: + Logger.localinfo(f"{self} - successfully {self._action_outcome}!") + self._return = 'done' + elif result.status in (ExecutionResult.PRECONDITION_FAILURE, + ExecutionResult.PLANNING_FAILURE, + ExecutionResult.EXECUTION_FAILURE): + Logger.logwarn(f"{self} : '{self._topic}' - failed to '{self._action_type}' command - '{result.message}'") + self._return = 'failed' + elif result.status in (ExecutionResult.CANCELED): + Logger.logwarn(f"{self} : '{self._topic}' - canceled '{self._action_type}' command -'{result.message}'") + self._return = 'failed' + else: + Logger.logwarn(f"{self} : '{self._topic}' -" + f" unknown failure to '{self._action_type}' command - ({result.status}) '{result.message}'") + self._return = 'failed' + return self._return + else: + Logger.logwarn(f"{self} : '{self._topic}' - command invalid action status to '{self._action_type}' '{result.message}'") + self._return = 'failed' + elif self._node.get_clock().now().nanoseconds - self._start_time.nanoseconds > self._timeout.nanoseconds: + # Failed to return call in timely manner + self._return = 'failed' + userdata.msg = f"{self._name}: failed to '{self._action_type}' within timeout!" + Logger.localwarn(userdata.msg) + + # Otherwise check for status change + if status == GoalStatus.STATUS_CANCELED: + Logger.loginfo(f" '{self}' : '{self._topic}' - goal was canceled! ") + userdata.msg = f"Request to '{self._action_type}' was canceled." + self._return = 'failed' + elif status == GoalStatus.STATUS_ABORTED: + Logger.loginfo(f" '{self}' : '{self._topic}' - goal was aborted! ") + userdata.msg = f"Request to '{self._action_type}' was aborted." + self._return = 'failed' + + # If the action has not yet finished, None outcome will be returned and the state stays active. + return self._return + + + def on_enter(self, userdata): + """Call when state becomes active.""" + # make sure to reset the error state since a previous state execution might have failed + self._return = None + self._client.remove_result(self._topic) # clear any prior result from action server + + # Send the goal. + try: + self._client.send_goal(self._topic, self._goal, wait_duration=self._timeout.nanoseconds*1e-9) + self._start_time = self._node.get_clock().now() + except Exception as exc: # pylint: disable=W0703 + # Since a state failure not necessarily causes a behavior failure, + # it is recommended to only print warnings, not errors. + # Using a linebreak before appending the error log enables the operator to collapse details in the GUI. + Logger.logwarn(f"Failed to send the '{self}' command:\n {type(exc)} - {exc}") + self._return = 'failed' + + def on_exit(self, userdata): + """Call when state is deactivated.""" + # Make sure that the action is not running when leaving this state. + # A situation where the action would still be active is for example when the operator manually triggers an outcome. + + if self._client.is_active(self._topic): + self._client.cancel(self._topic) + Logger.loginfo(f" '{self}' : '{self._topic}' -Cancelled active action goal.") + + # Local message are shown in terminal but not the UI + if self._return == 'done': + Logger.localinfo(f'Successfully completed pick action.') + else: + Logger.localwarn('Failed to complete pick action.') + + # Choosing to remove in on_enter and retain in proxy for now + # Either choice can be valid. + # if self._client.has_result(self._topic): + # # remove the old result so we are ready for the next time + # # and don't prematurely return + # self._client.remove_result(self._topic) diff --git a/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/follow_path_state.py b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/follow_path_state.py new file mode 100644 index 0000000..2c06ae5 --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/follow_path_state.py @@ -0,0 +1,165 @@ +#!/usr/bin/env python + +# Copyright 2024 Christopher Newport University +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""FlexBE State to command PyRoboSim robot to plan a path to target location.""" + +from action_msgs.msg import GoalStatus + +from flexbe_core import EventState, Logger +from flexbe_core.proxy import ProxyActionClient + +from pyrobosim_msgs.action import FollowPath +from pyrobosim_msgs.msg import ExecutionResult, Path + + + + +class FollowPathState(EventState): + """ + FlexBE state to request following a path for PyRoboSim robot + + Elements defined here for UI + Parameters + -- action_topic Action topic name (default= 'robot/follow_path') + -- server_timeout Wait for action server timeout in seconds (default = 5s) + + Outputs + <= done Successfully reached the goal + <= failed Failed to reach goal + + User data + ># path Path Desired path to follow + #> msg string Result message + """ + + def __init__(self, action_topic='robot/follow_path', + server_timeout=2.0): + # See example_state.py for basic explanations. + super().__init__(outcomes=['done', 'failed'], + input_keys=['path'], + output_keys=['msg']) + + self._goal = None + + self._topic = action_topic + self._server_timeout_sec = server_timeout + + self._client = ProxyActionClient({self._topic: FollowPath}, + wait_duration=0.0) # no need to wait here, we'll check on_enter + + self._return = None # Retain return value in case the outcome is blocked by operator + + def execute(self, userdata): + """ + Call periodically while this state is active. + + Check if the action has been finished and evaluate the result. + """ + if self._return is not None: + # Return prior outcome in case transition is blocked by autonomy level + return self._return + + status = self._client.get_status(self._topic) # get status before clearing result + if self._client.has_result(self._topic): + # Check if the action has been finished + result = self._client.get_result(self._topic, clear=True) + Logger.localinfo(f" '{self}' : '{self._topic}' returned {status} {result}") + if status == GoalStatus.STATUS_SUCCEEDED: + result_status = result.execution_result.status + userdata.msg = result.execution_result.message # Output message + if result_status == ExecutionResult.SUCCESS: + self._return = 'done' + else: + Logger.localwarn(f"{self} : '{self._topic}' -" + f" failed during follow ({result_status}) '{userdata.msg}'") + self._return = 'failed' + return self._return + # Note: Not checking a timeout here it is up to follow capability and/or operator to enforce + # given varying path lengths + + # Otherwise check for action status change + if status == GoalStatus.STATUS_CANCELED: + Logger.loginfo(f" '{self}' : '{self._topic}' - goal was canceled! ") + self._return = 'failed' + elif status == GoalStatus.STATUS_ABORTED: + Logger.loginfo(f" '{self}' : '{self._topic}' - goal was aborted! ") + self._return = 'failed' + + # If the action has not yet finished, None outcome will be returned and the state stays active. + return self._return + + + def on_enter(self, userdata): + """Call when state becomes active.""" + # make sure to reset the data from prior executions + self._return = None + userdata.msg = '' + self._client.remove_result(self._topic) # clear any prior result from action server + + if 'path' not in userdata: + self._return = 'failed' + userdata.msg = f"FollowPathState '{self}' requires userdata.path key!" + Logger.localwarn(userdata.msg) + return + + # Send the goal. + try: + path = userdata.path + if isinstance(path, Path): + self._goal = FollowPath.Goal(path=path) + else: + userdata.msg = f"Invalid goal type '{type(path)}' - must be pyrobosim_msgs/Path!" + Logger.localwarn(userdata.msg) + self._return = 'failed' + return + Logger.localinfo(f"Send follow path goal with {len(path.poses)} waypoints ...") + self._client.send_goal(self._topic, self._goal, wait_duration=self._server_timeout_sec ) + except Exception as exc: # pylint: disable=W0703 + # Since a state failure not necessarily causes a behavior failure, + # it is recommended to only print warnings, not errors. + # Using a linebreak before appending the error log enables the operator to collapse details in the GUI. + Logger.logwarn(f"Failed to send the '{self}' command:\n {type(exc)} - {exc}") + self._return = 'failed' + + def on_exit(self, userdata): + """Call when state is deactivated.""" + # Make sure that the action is not running when leaving this state. + # A situation where the action would still be active is for example when the operator manually triggers an outcome. + + if self._client.is_active(self._topic): + self._client.cancel(self._topic) + + # Check for action status change + status = self._client.get_status(self._topic) + if status == GoalStatus.STATUS_CANCELED: + Logger.loginfo(f" '{self}' : '{self._topic}' - request to follow was canceled! ") + self._return = 'failed' + else: + status_string = self._client.get_status_string(self._topic) + Logger.loginfo(f" '{self}' : '{self._topic}' - Requested to cancel an active follow request ({status_string}).") + + # Local message are shown in terminal but not the UI + if self._return == 'done': + Logger.localinfo(f'Successfully followed path.') + else: + Logger.localwarn('Failed to follow path.') + + # Choosing to remove in on_enter and retain in proxy for now + # Either choice can be valid. + # if self._client.has_result(self._topic): + # # remove the old result so we are ready for the next time + # # and don't prematurely return + # self._client.remove_result(self._topic) diff --git a/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/navigate_action_state.py b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/navigate_action_state.py new file mode 100644 index 0000000..b435ec6 --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/navigate_action_state.py @@ -0,0 +1,178 @@ +#!/usr/bin/env python + +# Copyright 2024 Christopher Newport University +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""FlexBE State to navigate PyRoboSim robot to target location.""" + +import math + +from action_msgs.msg import GoalStatus + +from rclpy.duration import Duration + +from flexbe_core import EventState, Logger +from flexbe_core.proxy import ProxyActionClient + +from pyrobosim_msgs.action import ExecuteTaskAction +from pyrobosim_msgs.msg import ExecutionResult, TaskAction + + + + +class NavigateActionState(EventState): + """ + FlexBE state to navigate PyRoboSim robot to target location + + Elements defined here for UI + Parameters + -- target_location Location to navigate to + -- robot_name Robot name (default= 'robot') + -- action_topic Action topic name (default= '/execute_action') + -- server_timeout Wait for server timeout in seconds (default = 5s) + -- navigate_timeout Timeout for action completion (default=None) + Outputs + <= done Navigation is complete + <= planning_failed Failed to plan path. + <= motion_failed Failed for some reason. + <= canceled User canceled before completion. + <= timeout Failed to navigate in timely manner + + #> msg Output message + """ + + def __init__(self, target_location, robot_name="robot", + action_topic='/execute_action', server_timeout=5.0, + navigate_timeout=None): + # See example_state.py for basic explanations. + super().__init__(outcomes=['done', 'planning_failed', 'motion_failed', 'canceled', 'timeout'], + input_keys=[], + output_keys=['msg']) + + self._goal = ExecuteTaskAction.Goal() + self._goal.action = TaskAction(robot=robot_name, + type="navigate", + target_location=target_location, + ) + + self._timeout = None + if navigate_timeout is not None: + self._timeout = Duration(seconds=navigate_timeout) + self._server_timeout_sec = float(server_timeout) + self._topic = action_topic + + self._client = ProxyActionClient({self._topic: ExecuteTaskAction}, + wait_duration=0.0) # pass required clients as dict (topic: type) + + # It may happen that the action client fails to send the action goal. + self._return = None # Retain return value in case the outcome is blocked by operator + self._start_time = None + self._elapsed = None + + def execute(self, userdata): + """ + Call periodically while this state is active. + + Check if the action has been finished and evaluate the result. + """ + if self._return is not None: + # Return prior outcome in case transition is blocked by autonomy level + return self._return + + status = self._client.get_status(self._topic) # get status before clearing result + if self._client.has_result(self._topic): + # Check if the action has been finished + result = self._client.get_result(self._topic, clear=True) + Logger.localinfo(f" '{self}' : '{self._topic}' returned {status} {result}") + if status == GoalStatus.STATUS_SUCCEEDED: + result = result.execution_result + self._elapsed = (self._node.get_clock().now() - self._start_time).nanoseconds*1e-9 + userdata.msg = result.message + + if result.status == ExecutionResult.SUCCESS: + self._return = 'done' + elif result.status in (ExecutionResult.PRECONDITION_FAILURE, + ExecutionResult.PLANNING_FAILURE): + Logger.logwarn(f"{self} : '{self._topic}' - planning failure '{result.message}'") + self._return = 'planning_failed' + elif result.status in (ExecutionResult.EXECUTION_FAILURE): + Logger.logwarn(f"{self} : '{self._topic}' - execution failure '{result.message}'") + self._return = 'motion_failed' + elif result.status in (ExecutionResult.CANCELED): + Logger.logwarn(f"{self} : '{self._topic}' - canceled '{result.message}'") + self._return = 'canceled' + else: + Logger.logwarn(f"{self} : '{self._topic}' -" + f" unknown failure ({result.status}) '{result.message}'") + self._return = 'motion_failed' + return self._return + + # Otherwise check for status change + if status == GoalStatus.STATUS_CANCELED: + Logger.loginfo(f" '{self}' : '{self._topic}' - goal was canceled! ") + self._return = 'canceled' + elif status == GoalStatus.STATUS_ABORTED: + Logger.loginfo(f" '{self}' : '{self._topic}' - goal was aborted! ") + self._return = 'canceled' + elif self._timeout is not None: + if self._node.get_clock().now().nanoseconds - self._start_time.nanoseconds > self._timeout.nanoseconds: + # Checking for timeout after we check for goal response + Logger.loginfo(f" '{self}' : '{self._topic}' - navigation timed out! ") + self._return = 'timeout' + + # If the action has not yet finished, None outcome will be returned and the state stays active. + return self._return + + + def on_enter(self, userdata): + """Call when state becomes active.""" + # make sure to reset the return state since a previous state execution might have failed + self._return = None + self._client.remove_result(self._topic) # clear any prior result from action server + + # Recording the start time to set motion duration output + self._start_time = self._node.get_clock().now() + + # Send the goal. + try: + Logger.loginfo(f"Request to navigate to '{self._goal.action.target_location}'") + self._client.send_goal(self._topic, self._goal, wait_duration=self._server_timeout_sec) + except Exception as exc: # pylint: disable=W0703 + # Since a state failure does not necessarily cause a behavior failure, + # it is recommended to only print warnings, not errors. + # Using a linebreak before appending the error log + # enables the operator to collapse details in the GUI. + Logger.logwarn(f"Failed to send the '{self}' command:\n {type(exc)} - {exc}") + self._return = 'planning_failed' + + def on_exit(self, userdata): + """Call when state is deactivated.""" + # Make sure that the action is not running when leaving this state. + # A situation where the action would still be active is for example when the operator manually triggers an outcome. + + if self._client.is_active(self._topic): + self._client.cancel(self._topic) + Logger.loginfo(f" '{self}' : '{self._topic}' -Cancelled active action goal.") + + if self._return == 'done': + Logger.loginfo(f'Successfully completed motion in {self._elapsed:.3f} seconds.') + else: + Logger.logwarn('Failed to complete motion.') + + # Choosing to remove in on_enter and retain in proxy for now + # Either choice can be valid. + # if self._client.has_result(self._topic): + # # remove the old result so we are ready for the next time + # # and don't prematurely return + # self._client.remove_result(self._topic) diff --git a/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/pick_action_state.py b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/pick_action_state.py new file mode 100644 index 0000000..294964f --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/pick_action_state.py @@ -0,0 +1,166 @@ +#!/usr/bin/env python + +# Copyright 2024 Christopher Newport University +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""FlexBE State to command PyRoboSim robot to pick up object at current location.""" + +from action_msgs.msg import GoalStatus + +from flexbe_core import EventState, Logger +from flexbe_core.proxy import ProxyActionClient + +from pyrobosim_msgs.action import ExecuteTaskAction +from pyrobosim_msgs.msg import ExecutionResult, TaskAction + +from rclpy.duration import Duration + + +class PickActionState(EventState): + """ + FlexBE state to pick action for PyRoboSim robot + + Elements defined here for UI + Parameters + -- robot_name Robot name (default= 'robot') + -- action_topic Action topic name (default= '/execute_action') + -- timeout Total time to wait for action to complete in seconds (default = 2s) + + Outputs + <= done Successfully grabbed object. + <= failed Failed to pick object + + User data + ># object string Desired object to pick up + #> msg string Result message + """ + + def __init__(self, robot_name="robot", + action_topic='/execute_action', + timeout=2.0): + # See example_state.py for basic explanations. + super().__init__(outcomes=['done', 'failed'], + input_keys=['object'], + output_keys=['msg']) + + self._goal = ExecuteTaskAction.Goal() + self._goal.action = TaskAction(robot=robot_name, type="pick") + + self._topic = action_topic + self._timeout = Duration(seconds=timeout) + + self._client = ProxyActionClient({self._topic: ExecuteTaskAction}, + wait_duration=0.0) # pass required clients as dict (topic: type) + + self._start_time = None + self._return = None # Retain return value in case the outcome is blocked by operator + + def execute(self, userdata): + """ + Call periodically while this state is active. + + Check if the action has been finished and evaluate the result. + """ + if self._return is not None: + # Return prior outcome in case transition is blocked by autonomy level + return self._return + + status = self._client.get_status(self._topic) # get status before clearing result + if self._client.has_result(self._topic): + # Check if the action has been finished + result = self._client.get_result(self._topic, clear=True).execution_result + userdata.msg = result.message # Output message + if status == GoalStatus.STATUS_SUCCEEDED: + if result.status == ExecutionResult.SUCCESS: + Logger.localinfo(f"{self} - successfully picked '{userdata.object}' ") + self._return = 'done' + elif result.status in (ExecutionResult.PRECONDITION_FAILURE, + ExecutionResult.PLANNING_FAILURE, + ExecutionResult.EXECUTION_FAILURE): + Logger.logwarn(f"{self} : '{self._topic}' - pick failure '{result.message}'") + self._return = 'failed' + elif result.status in (ExecutionResult.CANCELED): + Logger.logwarn(f"{self} : '{self._topic}' - canceled '{result.message}'") + self._return = 'failed' + else: + Logger.logwarn(f"{self} : '{self._topic}' -" + f" unknown failure ({result.status}) '{result.message}'") + self._return = 'failed' + return self._return + else: + Logger.logwarn(f"{self} : '{self._topic}' - invalid action status '{result.message}'") + self._return = 'failed' + elif self._node.get_clock().now().nanoseconds - self._start_time.nanoseconds > self._timeout.nanoseconds: + # Failed to return call in timely manner + self._return = 'failed' + userdata.msg = f"{self._name}: failed to pick object within timeout!" + Logger.localwarn(userdata.msg) + + # Otherwise check for status change + if status == GoalStatus.STATUS_CANCELED: + Logger.loginfo(f" '{self}' : '{self._topic}' - goal was canceled! ") + self._return = 'failed' + elif status == GoalStatus.STATUS_ABORTED: + Logger.loginfo(f" '{self}' : '{self._topic}' - goal was aborted! ") + self._return = 'failed' + + # If the action has not yet finished, None outcome will be returned and the state stays active. + return self._return + + + def on_enter(self, userdata): + """Call when state becomes active.""" + # make sure to reset the error state since a previous state execution might have failed + self._return = None + self._client.remove_result(self._topic) # clear any prior result from action server + + if 'object' not in userdata: + self._return = 'failed' + userdata.msg = f"PickActionState '{self}' requires userdata.object key!" + Logger.logwarn(userdata.msg) + return + + # Send the goal. + try: + self._goal.action.object = userdata.object + self._client.send_goal(self._topic, self._goal, wait_duration=self._timeout.nanoseconds*1e-9) + self._start_time = self._node.get_clock().now() + except Exception as exc: # pylint: disable=W0703 + # Since a state failure not necessarily causes a behavior failure, + # it is recommended to only print warnings, not errors. + # Using a linebreak before appending the error log enables the operator to collapse details in the GUI. + Logger.logwarn(f"Failed to send the '{self}' command:\n {type(exc)} - {exc}") + self._return = 'failed' + + def on_exit(self, userdata): + """Call when state is deactivated.""" + # Make sure that the action is not running when leaving this state. + # A situation where the action would still be active is for example when the operator manually triggers an outcome. + + if self._client.is_active(self._topic): + self._client.cancel(self._topic) + Logger.loginfo(f" '{self}' : '{self._topic}' -Cancelled active action goal.") + + # Local message are shown in terminal but not the UI + if self._return == 'done': + Logger.localinfo(f'Successfully completed pick action.') + else: + Logger.localwarn('Failed to complete pick action.') + + # Choosing to remove in on_enter and retain in proxy for now + # Either choice can be valid. + # if self._client.has_result(self._topic): + # # remove the old result so we are ready for the next time + # # and don't prematurely return + # self._client.remove_result(self._topic) diff --git a/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/place_action_state.py b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/place_action_state.py new file mode 100644 index 0000000..ca7e82a --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/place_action_state.py @@ -0,0 +1,160 @@ +#!/usr/bin/env python + +# Copyright 2024 Christopher Newport University +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""FlexBE State to command PyRoboSim robot to place up object at current location.""" + +from action_msgs.msg import GoalStatus + +from flexbe_core import EventState, Logger +from flexbe_core.proxy import ProxyActionClient + +from pyrobosim_msgs.action import ExecuteTaskAction +from pyrobosim_msgs.msg import ExecutionResult, TaskAction + +from rclpy.duration import Duration + + +class PlaceActionState(EventState): + """ + FlexBE state to place object action for PyRoboSim robot + + Elements defined here for UI + Parameters + -- robot_name Robot name (default= 'robot') + -- action_topic Action topic name (default= '/execute_action') + -- timeout Total time to wait for action to complete in seconds (default = 2s) + + Outputs + <= done Successfully grabbed object. + <= failed Failed to place object + + User data + #> msg string Result message + """ + + def __init__(self, robot_name='robot', + action_topic='/execute_action', + timeout=2.0): + # See example_state.py for basic explanations. + super().__init__(outcomes=['done', 'failed'], + input_keys=[], + output_keys=['msg']) + + self._goal = ExecuteTaskAction.Goal() + self._goal.action = TaskAction(robot=robot_name, + type="place", + ) + + self._topic = action_topic + self._timeout = Duration(seconds=timeout) + + self._client = ProxyActionClient({self._topic: ExecuteTaskAction}, + wait_duration=0.0) # pass required clients as dict (topic: type) + + self._return = None # Retain return value in case the outcome is blocked by operator + self._start_time = None + + def execute(self, userdata): + """ + Call periodically while this state is active. + + Check if the action has been finished and evaluate the result. + """ + if self._return is not None: + # Return prior outcome in case transition is blocked by autonomy level + return self._return + + status = self._client.get_status(self._topic) # get status before clearing result + if self._client.has_result(self._topic): + # Check if the action has been finished + result = self._client.get_result(self._topic, clear=True).execution_result + userdata.msg = result.message # Output message + if status == GoalStatus.STATUS_SUCCEEDED: + if result.status == ExecutionResult.SUCCESS: + Logger.localinfo(f"{self} - successfully placed object") + self._return = 'done' + elif result.status in (ExecutionResult.PRECONDITION_FAILURE, + ExecutionResult.PLANNING_FAILURE, + ExecutionResult.EXECUTION_FAILURE): + Logger.logwarn(f"{self} : '{self._topic}' - place failure '{result.message}'") + self._return = 'failed' + elif result.status in (ExecutionResult.CANCELED): + Logger.logwarn(f"{self} : '{self._topic}' - canceled '{result.message}'") + self._return = 'failed' + else: + Logger.logwarn(f"{self} : '{self._topic}' -" + f" unknown failure ({result.status}) '{result.message}'") + self._return = 'failed' + return self._return + else: + Logger.logwarn(f"{self} : '{self._topic}' - invalid action status '{userdata.msg}'") + self._return = 'failed' + elif self._node.get_clock().now().nanoseconds - self._start_time.nanoseconds > self._timeout.nanoseconds: + # Failed to return call in timely manner + self._return = 'failed' + userdata.msg = f"{self._name}: failed to place object within timeout!" + Logger.localwarn(userdata.msg) + + # Otherwise check for status change + if status == GoalStatus.STATUS_CANCELED: + Logger.loginfo(f" '{self}' : '{self._topic}' - goal was canceled! ") + self._return = 'failed' + elif status == GoalStatus.STATUS_ABORTED: + Logger.loginfo(f" '{self}' : '{self._topic}' - goal was aborted! ") + self._return = 'failed' + + # If the action has not yet finished, None outcome will be returned and the state stays active. + return self._return + + + def on_enter(self, userdata): + """Call when state becomes active.""" + # make sure to reset the error state since a previous state execution might have failed + self._return = None + self._client.remove_result(self._topic) # clear any prior result from action server + + # Send the goal. + try: + self._client.send_goal(self._topic, self._goal, wait_duration=self._timeout.nanoseconds*1e-9) + self._start_time = self._node.get_clock().now() + except Exception as exc: # pylint: disable=W0703 + # Since a state failure not necessarily causes a behavior failure, + # it is recommended to only print warnings, not errors. + # Using a linebreak before appending the error log enables the operator to collapse details in the GUI. + Logger.logwarn(f"Failed to send the '{self}' command:\n {type(exc)} - {exc}") + self._return = 'failed' + + def on_exit(self, userdata): + """Call when state is deactivated.""" + # Make sure that the action is not running when leaving this state. + # A situation where the action would still be active is for example when the operator manually triggers an outcome. + + if self._client.is_active(self._topic): + self._client.cancel(self._topic) + Logger.loginfo(f" '{self}' : '{self._topic}' -Cancelled active action goal.") + + # Local message are shown in terminal but not the UI + if self._return == 'done': + Logger.localinfo(f'Successfully completed place action.') + else: + Logger.localwarn('Failed to complete place action.') + + # Choosing to remove in on_enter and retain in proxy for now + # Either choice can be valid. + # if self._client.has_result(self._topic): + # # remove the old result so we are ready for the next time + # # and don't prematurely return + # self._client.remove_result(self._topic) diff --git a/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/plan_path_state.py b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/plan_path_state.py new file mode 100644 index 0000000..5228225 --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/plan_path_state.py @@ -0,0 +1,177 @@ +#!/usr/bin/env python + +# Copyright 2024 Christopher Newport University +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""FlexBE State to command PyRoboSim robot to plan a path to target location.""" + +from action_msgs.msg import GoalStatus + +from flexbe_core import EventState, Logger +from flexbe_core.proxy import ProxyActionClient + +from geometry_msgs.msg import Pose + +from pyrobosim_msgs.action import PlanPath +from pyrobosim_msgs.msg import ExecutionResult, Path + +from rclpy.duration import Duration + + +class PlanPathState(EventState): + """ + FlexBE state to request plan for PyRoboSim robot + + Elements defined here for UI + Parameters + -- action_topic Action topic name (default= 'robot/plan_path') + -- timeout Total time to wait for plan in seconds (default = 5s) + + Outputs + <= done Successfully planned path to goal. + <= failed Failed to plan path + + User data + ># goal string/Pose Desired object to pick up + #> msg string Result message + #> path Path Planned Path + """ + + def __init__(self, action_topic='robot/plan_path', + timeout=2.0): + # See example_state.py for basic explanations. + super().__init__(outcomes=['done', 'failed'], + input_keys=['goal'], + output_keys=['msg', 'path']) + + self._goal = None + + self._topic = action_topic + self._planning_timeout = Duration(seconds=timeout) + + self._client = ProxyActionClient({self._topic: PlanPath}, + wait_duration=0.0) # no need to wait here, we'll check on_enter + + self._return = None # Retain return value in case the outcome is blocked by operator + + def execute(self, userdata): + """ + Call periodically while this state is active. + + Check if the action has been finished and evaluate the result. + """ + if self._return is not None: + # Return prior outcome in case transition is blocked by autonomy level + return self._return + + status = self._client.get_status(self._topic) # get status before clearing result + if self._client.has_result(self._topic): + # Check if the action has been finished + result = self._client.get_result(self._topic, clear=True) + userdata.msg = result.execution_result.message # Output message + if status == GoalStatus.STATUS_SUCCEEDED: + result_status = result.execution_result.status + userdata.path = None + if result_status == ExecutionResult.SUCCESS and result.path is not None: + Logger.localinfo(f"{self} - planning success with {len(result.path.poses)} waypoints") + userdata.path = result.path + self._return = 'done' + else: + Logger.localwarn(f"{self} : '{self._topic}' -" + f" planning failure ({result_status}) '{userdata.msg}'") + self._return = 'failed' + return self._return + else: + Logger.logwarn(f"{self} : '{self._topic}' - invalid action status '{userdata.msg}'") + self._return = 'failed' + elif self._node.get_clock().now().nanoseconds - self._start_time.nanoseconds > self._planning_timeout.nanoseconds: + # Failed to return call in timely manner + self._return = 'failed' + userdata.msg = f"{self._name}: failed to plan path within timeout!" + Logger.localwarn(userdata.msg) + + # Otherwise check for action status change + if status == GoalStatus.STATUS_CANCELED: + Logger.loginfo(f" '{self}' : '{self._topic}' - goal was canceled! ") + self._return = 'failed' + elif status == GoalStatus.STATUS_ABORTED: + Logger.loginfo(f" '{self}' : '{self._topic}' - goal was aborted! ") + self._return = 'failed' + + # If the action has not yet finished, None outcome will be returned and the state stays active. + return self._return + + + def on_enter(self, userdata): + """Call when state becomes active.""" + # make sure to reset the data from prior executions + self._return = None + userdata.path = None + userdata.msg = '' + self._client.remove_result(self._topic) # clear any prior result from action server + if 'goal' not in userdata: + self._return = 'failed' + userdata.msg = f"PlanActionState '{self}' requires userdata.goal key!" + Logger.localwarn(userdata.msg) + return + + # Send the goal. + try: + goal = userdata.goal + if isinstance(goal, str): + self._goal = PlanPath.Goal(target_location = goal) + elif isinstance(goal, Pose): + self._goal = PlanPath.Goal(pose=goal) + else: + userdata.msg = f"Invalid goal type '{type(goal)}' - must be string or Pose!" + Logger.localwarn(userdata.msg) + self._return = 'failed' + return + self._client.send_goal(self._topic, self._goal, wait_duration=self._planning_timeout.nanoseconds*1e-9) + self._start_time = self._node.get_clock().now() + except Exception as exc: # pylint: disable=W0703 + # Since a state failure not necessarily causes a behavior failure, + # it is recommended to only print warnings, not errors. + # Using a linebreak before appending the error log enables the operator to collapse details in the GUI. + Logger.logwarn(f"Failed to send the '{self}' command:\n {type(exc)} - {exc}") + self._return = 'failed' + + def on_exit(self, userdata): + """Call when state is deactivated.""" + # Make sure that the action is not running when leaving this state. + # A situation where the action would still be active is for example when the operator manually triggers an outcome. + + if self._client.is_active(self._topic): + self._client.cancel(self._topic) + # Check for action status change + status = self._client.get_status(self._topic) # get status before clearing result + if status == GoalStatus.STATUS_CANCELED: + Logger.loginfo(f" '{self}' : '{self._topic}' - request to plan was canceled! ") + self._return = 'failed' + else: + status_string = self._client.get_status_string(self._topic) + Logger.loginfo(f" '{self}' : '{self._topic}' - Requested to cancel an active plan request ({status_string}).") + + # Local message are shown in terminal but not the UI + if self._return == 'done': + Logger.localinfo(f'Successfully planned path.') + else: + Logger.localwarn('Failed to plan path.') + + # Choosing to remove in on_enter and retain in proxy for now + # Either choice can be valid. + # if self._client.has_result(self._topic): + # # remove the old result so we are ready for the next time + # # and don't prematurely return + # self._client.remove_result(self._topic) diff --git a/technologies/FlexBE/pyrobosim_flexbe_states/resource/pyrobosim_flexbe_states b/technologies/FlexBE/pyrobosim_flexbe_states/resource/pyrobosim_flexbe_states new file mode 100644 index 0000000..e69de29 diff --git a/technologies/FlexBE/pyrobosim_flexbe_states/setup.cfg b/technologies/FlexBE/pyrobosim_flexbe_states/setup.cfg new file mode 100644 index 0000000..86b6087 --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_states/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/pyrobosim_flexbe_states +[install] +install_scripts=$base/lib/pyrobosim_flexbe_states diff --git a/technologies/FlexBE/pyrobosim_flexbe_states/setup.py b/technologies/FlexBE/pyrobosim_flexbe_states/setup.py new file mode 100644 index 0000000..751d489 --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_states/setup.py @@ -0,0 +1,34 @@ +#!/usr/bin/env python + +from glob import glob +from setuptools import setup +from setuptools import find_packages + +PACKAGE_NAME = 'pyrobosim_flexbe_states' + +setup( + name=PACKAGE_NAME, + version='0.0.1', + packages=find_packages(), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + PACKAGE_NAME]), + ('share/' + PACKAGE_NAME, ['package.xml']), + ('share/' + PACKAGE_NAME + "/tests", glob('tests/*.test')), + ('share/' + PACKAGE_NAME + "/launch", glob('tests/*.launch.py')), + ], + + install_requires=['setuptools'], + zip_safe=True, + maintainer='TODO', + maintainer_email='TODO@TODO.com', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'example_action_state = pyrobosim_flexbe_states.example_action_state', + 'example_state = pyrobosim_flexbe_states.example_state', + ], + }, +) diff --git a/technologies/FlexBE/pyrobosim_flexbe_states/tests/launch_test.py b/technologies/FlexBE/pyrobosim_flexbe_states/tests/launch_test.py new file mode 100644 index 0000000..1a9bc72 --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_states/tests/launch_test.py @@ -0,0 +1,64 @@ +# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University +# +# 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 Philipp Schillinger, Team ViGIR, Christopher Newport University 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. + +"""pyrobosim_flexbe_states testing.""" + +from os.path import join + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.substitutions import LaunchConfiguration +from launch.launch_description_sources import PythonLaunchDescriptionSource +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + """Flexbe_states testing.""" + flexbe_testing_dir = get_package_share_directory('flexbe_testing') + flexbe_states_test_dir = get_package_share_directory('pyrobosim_flexbe_states') + + path = join(flexbe_states_test_dir, "tests") + + # The tests + testcases = "" + testcases += join(path, "example_state.test") + "\n" + testcases += join(path, "example_action_state.test") + "\n" + + return LaunchDescription([ + DeclareLaunchArgument("pkg", default_value="pyrobosim_flexbe_states"), + DeclareLaunchArgument("testcases", default_value=testcases), + DeclareLaunchArgument("compact_format", default_value='true'), + IncludeLaunchDescription( + PythonLaunchDescriptionSource(join(flexbe_testing_dir, "launch", "flexbe_testing.launch.py")), + launch_arguments={ + 'package': LaunchConfiguration("pkg"), + 'compact_format': LaunchConfiguration("compact_format"), + 'testcases': LaunchConfiguration("testcases"), + }.items() + ) + ]) diff --git a/technologies/FlexBE/pyrobosim_flexbe_states/tests/run_colcon_test.py b/technologies/FlexBE/pyrobosim_flexbe_states/tests/run_colcon_test.py new file mode 100644 index 0000000..9b20f19 --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_states/tests/run_colcon_test.py @@ -0,0 +1,51 @@ +# Copyright 2023 Christopher Newport University +# +# 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 Philipp Schillinger, Team ViGIR, Christopher Newport University 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. + + +"""Pytest testing for pyrobosim_flexbe_states.""" + + +from flexbe_testing.py_tester import PyTester + + +class TestFlexBEStates(PyTester): + """Pytest testing for pyrobosim_flexbe_states.""" + + # def __init__(self, *args, **kwargs): + # """Initialize unit test.""" + # super().__init__(*args, **kwargs) + + @classmethod + def setUpClass(cls): + + PyTester._package = "pyrobosim_flexbe_states" + PyTester._tests_folder = "tests" + + PyTester.setUpClass() # Do this last after setting package and tests folder + + # The tests - to do add tests From 7d075b1871675557252bc905ce4cf03ed8c6bcc4 Mon Sep 17 00:00:00 2001 From: dcconner Date: Fri, 6 Sep 2024 13:19:00 -0400 Subject: [PATCH 24/51] update fastapi requirements (#23) --- dependencies/FlexBE/flexbe_webui | 2 +- python-requirements.txt | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/dependencies/FlexBE/flexbe_webui b/dependencies/FlexBE/flexbe_webui index f18cece..85a4b89 160000 --- a/dependencies/FlexBE/flexbe_webui +++ b/dependencies/FlexBE/flexbe_webui @@ -1 +1 @@ -Subproject commit f18cece1f85ab126370db042958ba1beeebab474 +Subproject commit 85a4b897b390c004fb4bb7093e61ca64001baa8b diff --git a/python-requirements.txt b/python-requirements.txt index 1fa147c..367a089 100644 --- a/python-requirements.txt +++ b/python-requirements.txt @@ -1,6 +1,6 @@ adjustText astar -fastAPI==0.89.1 +fastAPI>=0.109.1 matplotlib numpy<2.1.0 pycollada From 41ac382442401133697126621d360ea1179f5b8e Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Sat, 7 Sep 2024 17:43:25 +0200 Subject: [PATCH 25/51] Preliminary integration of BTCPP (#25) --- .gitignore | 3 + .gitmodules | 4 + .pre-commit-config.yaml | 8 ++ technologies/BehaviorTree.CPP/README.md | 31 ++++++ .../pyrobosim_btcpp/.clang-format | 68 ++++++++++++ .../pyrobosim_btcpp/CMakeLists.txt | 44 ++++++++ .../pyrobosim_btcpp/nodes/close_door_node.hpp | 0 .../nodes/close_fridge_node.hpp | 0 .../nodes/detect_object_node.hpp | 0 .../nodes/execute_task_node.hpp | 100 ++++++++++++++++++ .../nodes/get_current_location_node.hpp | 0 .../pyrobosim_btcpp/nodes/navigate_node.hpp | 36 +++++++ .../pyrobosim_btcpp/nodes/open_door_node.hpp | 0 .../nodes/open_fridge_node.hpp | 0 .../nodes/pick_object_node.hpp | 0 .../nodes/place_object_node.hpp | 0 .../pyrobosim_btcpp/package.xml | 23 ++++ .../pyrobosim_btcpp/src/btcpp_executor.cpp | 88 +++++++++++++++ .../pyrobosim_btcpp/trees/navigation_demo.xml | 11 ++ 19 files changed, 416 insertions(+) create mode 100644 technologies/BehaviorTree.CPP/README.md create mode 100644 technologies/BehaviorTree.CPP/pyrobosim_btcpp/.clang-format create mode 100644 technologies/BehaviorTree.CPP/pyrobosim_btcpp/CMakeLists.txt create mode 100644 technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/close_door_node.hpp create mode 100644 technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/close_fridge_node.hpp create mode 100644 technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/detect_object_node.hpp create mode 100644 technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/execute_task_node.hpp create mode 100644 technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/get_current_location_node.hpp create mode 100644 technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/navigate_node.hpp create mode 100644 technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/open_door_node.hpp create mode 100644 technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/open_fridge_node.hpp create mode 100644 technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/pick_object_node.hpp create mode 100644 technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/place_object_node.hpp create mode 100644 technologies/BehaviorTree.CPP/pyrobosim_btcpp/package.xml create mode 100644 technologies/BehaviorTree.CPP/pyrobosim_btcpp/src/btcpp_executor.cpp create mode 100644 technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/navigation_demo.xml diff --git a/.gitignore b/.gitignore index 45436f6..f699007 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,6 @@ __pycache__/ .colcon/ .vscode +/build/* +/install/* +/log/* diff --git a/.gitmodules b/.gitmodules index fb4249a..0fcff78 100644 --- a/.gitmodules +++ b/.gitmodules @@ -30,3 +30,7 @@ path = dependencies/FlexBE/flexbe_webui url = https://github.com/FlexBE/flexbe_webui.git branch = main +[submodule "dependencies/BehaviorTree.ROS2"] + path = dependencies/BehaviorTree.ROS2 + url = https://github.com/BehaviorTree/BehaviorTree.ROS2.git + branch = humble diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 241e4f8..f3317f1 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -61,4 +61,12 @@ repos: hooks: - id: markdown-link-check + # CPP formatting + - repo: https://github.com/pre-commit/mirrors-clang-format + rev: v17.0.6 + hooks: + - id: clang-format + types_or: [c++, c] + args: ['-fallback-style=none', '-i'] + exclude: '(dependencies|.colcon)/.*' diff --git a/technologies/BehaviorTree.CPP/README.md b/technologies/BehaviorTree.CPP/README.md new file mode 100644 index 0000000..5a7c6eb --- /dev/null +++ b/technologies/BehaviorTree.CPP/README.md @@ -0,0 +1,31 @@ +# BehaviorTree.CPP Nodes for ROSCon 2024 Deliberation Technologies Workshop + +This package provides some ready to use Nodes (i.e. "actions") to be used with **pyrobosim**. + +## How to run + +A BTCPP executor is provided to run Behavior Trees stored as XML files + +For instance, consider this sample XML in [trees/navigation_demo.xml](pyrobosim_btcpp/trees/navigation_demo.xml) + +```xml + + + + + + + + + + + +``` + +NOTE: remember that the `name` attribute in the XML is optional and used for debugging only. + +You can run the BeahviorTree with the command: + +``` +ros2 run pyrobosim_btcpp btcpp_executor --ros-args -p tree:=trees/navigation_demo.xml +``` diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/.clang-format b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/.clang-format new file mode 100644 index 0000000..e0f2099 --- /dev/null +++ b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/.clang-format @@ -0,0 +1,68 @@ +--- +BasedOnStyle: Google +AccessModifierOffset: -2 +ConstructorInitializerIndentWidth: 2 +AlignEscapedNewlinesLeft: false +AlignTrailingComments: true +AllowAllParametersOfDeclarationOnNextLine: false +AllowShortIfStatementsOnASingleLine: false +AllowShortLoopsOnASingleLine: false +AllowShortFunctionsOnASingleLine: None +AlwaysBreakTemplateDeclarations: true +AlwaysBreakBeforeMultilineStrings: false +BreakBeforeBinaryOperators: false +BreakBeforeTernaryOperators: false +BreakConstructorInitializers: BeforeComma +BinPackParameters: true +ColumnLimit: 100 +ConstructorInitializerAllOnOneLineOrOnePerLine: true +DerivePointerBinding: false +PointerBindsToType: true +ExperimentalAutoDetectBinPacking: false +IndentCaseLabels: true +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCSpaceBeforeProtocolList: true +PenaltyBreakBeforeFirstCallParameter: 19 +PenaltyBreakComment: 60 +PenaltyBreakString: 1 +PenaltyBreakFirstLessLess: 1000 +PenaltyExcessCharacter: 1000 +PenaltyReturnTypeOnItsOwnLine: 90 +SpacesBeforeTrailingComments: 2 +Cpp11BracedListStyle: false +Standard: Auto +IndentWidth: 2 +TabWidth: 2 +UseTab: Never +IndentFunctionDeclarationAfterType: false +SpacesInParentheses: false +SpacesInAngles: false +SpaceInEmptyParentheses: false +SpacesInCStyleCastParentheses: false +SpaceAfterControlStatementKeyword: true +SpaceBeforeAssignmentOperators: true +SpaceBeforeParens: Never +ContinuationIndentWidth: 4 +SortIncludes: false +SpaceAfterCStyleCast: false +ReflowComments: false + +# Configure each individual brace in BraceWrapping +BreakBeforeBraces: Custom + +# Control of individual brace wrapping cases +BraceWrapping: { + AfterClass: 'true', + AfterControlStatement: 'true', + AfterEnum : 'true', + AfterFunction : 'true', + AfterNamespace : 'true', + AfterStruct : 'true', + AfterUnion : 'true', + BeforeCatch : 'true', + BeforeElse : 'true', + IndentBraces : 'false', + SplitEmptyFunction: 'false' +} +... diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/CMakeLists.txt b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/CMakeLists.txt new file mode 100644 index 0000000..bd47242 --- /dev/null +++ b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/CMakeLists.txt @@ -0,0 +1,44 @@ +cmake_minimum_required(VERSION 3.8) +project(pyrobosim_btcpp) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) + +set(THIS_PACKAGE_DEPS + ament_index_cpp + behaviortree_ros2 + pyrobosim_msgs + rclcpp +) + +# for each dependency, find and include the package +foreach(DEPENDENCY ${THIS_PACKAGE_DEPS}) + find_package(${DEPENDENCY} REQUIRED) +endforeach() + +# create executable +add_executable(btcpp_executor src/btcpp_executor.cpp) + +ament_target_dependencies(btcpp_executor ${THIS_PACKAGE_DEPS}) + +target_include_directories(btcpp_executor PUBLIC + $ + $) + + +# Install executable and trees +install( + DIRECTORY trees + DESTINATION share/${PROJECT_NAME} +) + +install( + TARGETS btcpp_executor + DESTINATION DESTINATION lib/${PROJECT_NAME} +) + +ament_package() diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/close_door_node.hpp b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/close_door_node.hpp new file mode 100644 index 0000000..e69de29 diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/close_fridge_node.hpp b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/close_fridge_node.hpp new file mode 100644 index 0000000..e69de29 diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/detect_object_node.hpp b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/detect_object_node.hpp new file mode 100644 index 0000000..e69de29 diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/execute_task_node.hpp b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/execute_task_node.hpp new file mode 100644 index 0000000..562515a --- /dev/null +++ b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/execute_task_node.hpp @@ -0,0 +1,100 @@ +#include +#include + +namespace BT +{ + +inline const char* resultToStr(pyrobosim_msgs::msg::ExecutionResult result) +{ + switch(result.status) + { + case pyrobosim_msgs::msg::ExecutionResult::UNKNOWN: + return "UNKNOWN"; + case pyrobosim_msgs::msg::ExecutionResult::SUCCESS: + return "SUCCESS"; + case pyrobosim_msgs::msg::ExecutionResult::PRECONDITION_FAILURE: + return "PRECONDITION_FAILURE"; + case pyrobosim_msgs::msg::ExecutionResult::PLANNING_FAILURE: + return "PLANNING_FAILURE"; + case pyrobosim_msgs::msg::ExecutionResult::EXECUTION_FAILURE: + return "EXECUTION_FAILURE"; + case pyrobosim_msgs::msg::ExecutionResult::POSTCONDITION_FAILURE: + return "POSTCONDITION_FAILURE"; + case pyrobosim_msgs::msg::ExecutionResult::INVALID_ACTION: + return "INVALID_ACTION"; + case pyrobosim_msgs::msg::ExecutionResult::CANCELED: + return "CANCELED"; + } + return "UNKNOWN"; +} + +/** + * @brief Base class for all the nodes that execute a task using the ExecuteTaskAction + */ +class ExecuteTaskNode : public RosActionNode +{ +public: + ExecuteTaskNode(const std::string& name, const NodeConfig& conf, const RosNodeParams& params) + : RosActionNode(name, conf, params) + {} + + using TaskAction = pyrobosim_msgs::msg::TaskAction; + using ExecutionResult = pyrobosim_msgs::msg::ExecutionResult; + + // method that sends the goal. Must be implemented by the derived class + virtual bool setGoal(TaskAction& action) = 0; + + // virtual method processing the result. Can be overridden by the derived class + virtual NodeStatus onResultReceived(const ExecutionResult& execution_result); + + // default implementation of the onFailure callback. Can be overridden by the derived class + NodeStatus onFailure(ActionNodeErrorCode error) override; + + // this helper function will add the port "robotID" and "action_name" by default to every node. + // They both have default values and don't need to be specified in the XML files + static BT::PortsList appendProvidedPorts(PortsList other_ports); + +private: + bool setGoal(Goal& goal) override final + { + // initialize the field goal.action.robot + getInput("robotID", goal.action.robot); + return setGoal(goal.action); + } + + NodeStatus onResultReceived(const WrappedResult& wr) override final + { + return onResultReceived(wr.result->execution_result); + } +}; + +//------------------------------------------------------------ +//------------------------------------------------------------ +//------------------------------------------------------------ + +inline NodeStatus ExecuteTaskNode::onResultReceived(const ExecutionResult& execution_result) +{ + if(execution_result.status != ExecutionResult::SUCCESS) + { + RCLCPP_ERROR(logger(), "[%s] failed with error: %s. Message: %s", name().c_str(), + resultToStr(execution_result), execution_result.message.c_str()); + return NodeStatus::FAILURE; + } + return NodeStatus::SUCCESS; +} + +inline NodeStatus ExecuteTaskNode::onFailure(ActionNodeErrorCode error) +{ + RCLCPP_ERROR(logger(), "[%s] failed with error: %s", name().c_str(), toStr(error)); + return NodeStatus::FAILURE; +} + +inline BT::PortsList ExecuteTaskNode::appendProvidedPorts(PortsList other_ports) +{ + PortsList ports = { InputPort("action_name", "execute_action", "Action server name"), + InputPort("robotID", "robot", "Robot name") }; + ports.insert(other_ports.begin(), other_ports.end()); + return ports; +} + +} // namespace BT diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/get_current_location_node.hpp b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/get_current_location_node.hpp new file mode 100644 index 0000000..e69de29 diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/navigate_node.hpp b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/navigate_node.hpp new file mode 100644 index 0000000..1e0e629 --- /dev/null +++ b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/navigate_node.hpp @@ -0,0 +1,36 @@ +#include +#include +#include "execute_task_node.hpp" + +namespace BT +{ + +class NavigateAction : public ExecuteTaskNode +{ +public: + NavigateAction(const std::string& name, const NodeConfig& conf, const RosNodeParams& params) + : ExecuteTaskNode(name, conf, params) + {} + + // specify the ports offered by this node + static BT::PortsList providedPorts() + { + return ExecuteTaskNode::appendProvidedPorts({ BT::InputPort("target") }); + } + + // Implement the method that sends the goal + bool setGoal(TaskAction& action) override + { + std::string location; + if(!getInput("target", location) || location.empty()) + { + throw BT::RuntimeError("missing required input [target]"); + } + // prepare the goal message + action.type = "navigate"; + action.target_location = location; + return true; + } +}; + +} // namespace BT diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/open_door_node.hpp b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/open_door_node.hpp new file mode 100644 index 0000000..e69de29 diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/open_fridge_node.hpp b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/open_fridge_node.hpp new file mode 100644 index 0000000..e69de29 diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/pick_object_node.hpp b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/pick_object_node.hpp new file mode 100644 index 0000000..e69de29 diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/place_object_node.hpp b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/place_object_node.hpp new file mode 100644 index 0000000..e69de29 diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/package.xml b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/package.xml new file mode 100644 index 0000000..9c1180f --- /dev/null +++ b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/package.xml @@ -0,0 +1,23 @@ + + + + pyrobosim_btcpp + 0.0.0 + TODO: Package description + root + TODO: License declaration + + ament_cmake + + ament_index_cpp + behaviortree_ros2 + pyrobosim_msgs + rclcpp + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/src/btcpp_executor.cpp b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/src/btcpp_executor.cpp new file mode 100644 index 0000000..cfc32d5 --- /dev/null +++ b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/src/btcpp_executor.cpp @@ -0,0 +1,88 @@ +#include + +// ROS includes +#include +#include + +// BTCPP includes +#include +#include +#include + +// BTCPP nodes in this package +#include "pyrobosim_btcpp/nodes/navigate_node.hpp" + +std::filesystem::path GetFilePath(const std::string& filename) +{ + // check first the given path + if(std::filesystem::exists(filename)) + { + return filename; + } + // try appending the package directory + const std::string package_dir = ament_index_cpp::get_package_share_directory("pyrobosim_btcpp"); + const auto package_path = std::filesystem::path(package_dir) / filename; + if(std::filesystem::exists(package_path)) + { + return package_path; + } + throw std::runtime_error("File not found: " + filename); +} + +int main(int argc, char** argv) +{ + // Create a ROS Node + rclcpp::init(argc, argv); + auto nh = std::make_shared("btcpp_executor"); + + nh->declare_parameter("tree", rclcpp::PARAMETER_STRING); + nh->declare_parameter("save-model", false); + + const std::string tree_filename = nh->get_parameter("tree").as_string(); + const bool save_model = nh->get_parameter("save-model").as_bool(); + + if(tree_filename.empty()) + { + RCLCPP_FATAL(nh->get_logger(), "Missing parameter 'tree' with the path to the Behavior Tree " + "XML file"); + return 1; + } + const std::filesystem::path filepath = GetFilePath(tree_filename); + + //---------------------------------- + // register all the actions in the factory + BT::BehaviorTreeFactory factory; + + // all the actions are done using the same Action Server. + // Therefore single RosNodeParams will do. + BT::RosNodeParams params; + params.nh = nh; + params.default_port_value = "execute_action"; + + factory.registerNodeType("Navigate", params); + + // optionally we can display and save the model of the tree + if(save_model) + { + std::string xml_models = BT::writeTreeNodesModelXML(factory); + std::cout << "--------- Node Models ---------:\n" << xml_models << std::endl; + std::ofstream of("tree_nodes_model.xml"); + of << xml_models; + std::cout << "\nXML model of the tree saved in tree_nodes_model.xml\n" << std::endl; + } + + //---------------------------------- + // load a tree and execute + BT::Tree tree = factory.createTreeFromFile(filepath.string()); + + // This will add console messages for each action and condition executed + BT::StdCoutLogger console_logger(tree); + console_logger.enableTransitionToIdle(false); + + // This is the "main loop":xecution is completed once the tick() method returns SUCCESS of FAILURE + BT::NodeStatus res = tree.tickWhileRunning(); + + std::cout << "Execution completed. Result: " << BT::toStr(res) << std::endl; + + return 0; +} diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/navigation_demo.xml b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/navigation_demo.xml new file mode 100644 index 0000000..a81bfc0 --- /dev/null +++ b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/navigation_demo.xml @@ -0,0 +1,11 @@ + + + + + + + + + + + From a3d0b6b3147bca3f505b6f4da18bfa432d1d06d0 Mon Sep 17 00:00:00 2001 From: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Date: Sat, 7 Sep 2024 23:05:04 -0400 Subject: [PATCH 26/51] Actually add BehaviorTree.ROS submodule (#31) --- .gitmodules | 6 +++--- dependencies/BehaviorTree.ROS2 | 1 + 2 files changed, 4 insertions(+), 3 deletions(-) create mode 160000 dependencies/BehaviorTree.ROS2 diff --git a/.gitmodules b/.gitmodules index 0fcff78..89b07c2 100644 --- a/.gitmodules +++ b/.gitmodules @@ -22,15 +22,15 @@ path = technologies/SkiROS2/skiros2_pyrobosim_lib url = https://github.com/matthias-mayr/skiros2_pyrobosim_lib.git branch = main -[submodule "dependencies/FlexBE/flexbe_behavior_engine"] +[submodule "flexbe_behavior_engine"] path = dependencies/FlexBE/flexbe_behavior_engine url = https://github.com/FlexBE/flexbe_behavior_engine.git branch = ros2-devel -[submodule "dependencies/FlexBE/flexbe_webui"] +[submodule "flexbe_webui"] path = dependencies/FlexBE/flexbe_webui url = https://github.com/FlexBE/flexbe_webui.git branch = main -[submodule "dependencies/BehaviorTree.ROS2"] +[submodule "BehaviorTree.ROS2"] path = dependencies/BehaviorTree.ROS2 url = https://github.com/BehaviorTree/BehaviorTree.ROS2.git branch = humble diff --git a/dependencies/BehaviorTree.ROS2 b/dependencies/BehaviorTree.ROS2 new file mode 160000 index 0000000..adec04b --- /dev/null +++ b/dependencies/BehaviorTree.ROS2 @@ -0,0 +1 @@ +Subproject commit adec04ba58531de37f87328632a72a506cc6b0d4 From 1ae23ed06c93db78a12a2068f0c5625db55242a5 Mon Sep 17 00:00:00 2001 From: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Date: Sun, 8 Sep 2024 06:17:59 -0400 Subject: [PATCH 27/51] Update pyrobosim submodule (#32) Closes #28 about errors when placing objects that the robot does not have Closes #30 about the introduction of timestamps --- dependencies/pyrobosim | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dependencies/pyrobosim b/dependencies/pyrobosim index ce53680..929e04b 160000 --- a/dependencies/pyrobosim +++ b/dependencies/pyrobosim @@ -1 +1 @@ -Subproject commit ce53680d315181161cb86bfb6b6f28d6f7769a41 +Subproject commit 929e04b454096240013b692f34b927952a2d0b95 From 1071108d89409fce6a79bfa6bae6bf21ab489818 Mon Sep 17 00:00:00 2001 From: Matthias Mayr Date: Sun, 8 Sep 2024 20:10:47 +0200 Subject: [PATCH 28/51] New: SkiROS2 knowledge modeling and problem 1 update (#34) --- dependencies/SkiROS2/skiros2_std_lib | 2 +- technologies/SkiROS2/skiros2_pyrobosim_lib | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/dependencies/SkiROS2/skiros2_std_lib b/dependencies/SkiROS2/skiros2_std_lib index be8b027..d80e55e 160000 --- a/dependencies/SkiROS2/skiros2_std_lib +++ b/dependencies/SkiROS2/skiros2_std_lib @@ -1 +1 @@ -Subproject commit be8b027c6213936d57d8f6491814dddebb707cf2 +Subproject commit d80e55e2afd90fe49e6f8b5d1c02afa664e21955 diff --git a/technologies/SkiROS2/skiros2_pyrobosim_lib b/technologies/SkiROS2/skiros2_pyrobosim_lib index ed5336b..f8eed45 160000 --- a/technologies/SkiROS2/skiros2_pyrobosim_lib +++ b/technologies/SkiROS2/skiros2_pyrobosim_lib @@ -1 +1 @@ -Subproject commit ed5336b3aff9f85a20deb6b5dfac1537b337bd7a +Subproject commit f8eed45e02ef93c0214f3dd2791f7d7b6b4a1e74 From 8962ed7bc2c3dd80f623e262a4e81651c2d8ceaf Mon Sep 17 00:00:00 2001 From: Matthias Mayr Date: Sun, 8 Sep 2024 23:37:18 +0200 Subject: [PATCH 29/51] New: Adds convenience functions and python warning suppression (#27) * New: Adds convenience functions and python warning suppression * New: Allows to build single package and up to a package * New: Introduces WORKSPACE_ROOT and uses ROS_WS more --- .docker/entrypoint.sh | 62 ++++++++++++++++++++++++++++++++++++++++--- 1 file changed, 58 insertions(+), 4 deletions(-) diff --git a/.docker/entrypoint.sh b/.docker/entrypoint.sh index e6c3a95..8d3998f 100755 --- a/.docker/entrypoint.sh +++ b/.docker/entrypoint.sh @@ -2,12 +2,66 @@ # Source ROS and the deliberation workspace. source /opt/ros/jazzy/setup.bash -if [ ! -f /delib_ws/install/setup.bash ] + +# Ignores additional output because of deprecated Python tooling in ament_python +export PYTHONWARNINGS="ignore:setup.py install is deprecated,ignore:easy_install command is deprecated" + +export ROS_WS=/delib_ws +export WORKSPACE_ROOT=$ROS_WS # For FlexBE compatibility +export ROS_DOMAIN_ID=0 + +function print_ros_variables () { + echo -e "ROS Distro: \t" $ROS_DISTRO + echo -e "ROS Domain ID: \t" $ROS_DOMAIN_ID + echo -e "ROS Workspace: \t" $ROS_WS +} +print_ros_variables + +# Convenience functions +alias delib_src='cd $ROS_WS/src' +alias delib_ws='cd $ROS_WS' +# TODO(matthias-mayr): This ignored package is still being ported. +function delib_build() { + cwd=$(pwd) + delib_ws + colcon build --symlink-install --continue-on-error --packages-ignore skiros2_task + cd $cwd +} + +function delib_build_packages() { + cwd=$(pwd) + delib_ws + colcon build --symlink-install --continue-on-error --packages-ignore skiros2_task --packages-select "$@" + cd $cwd +} + +function delib_build_packages_up_to() { + cwd=$(pwd) + delib_ws + colcon build --symlink-install --continue-on-error --packages-ignore skiros2_task --packages-up-to "$@" + cd $cwd +} + +function delib_clean () { + cwd=$(pwd) + delib_ws + local _ret=$? + if [ $_ret -ne 0 ] ; then + echo "Could not switch dirs. Aborting delib_clean" + return $_ret + fi + rm -r build/* + rm -r install/* + rm -r log/* + cd $cwd +} + +# Automatic build when entering the container +if [ ! -f $ROS_WS/install/setup.bash ] then - # TODO(matthias-mayr): This ignored package is still being ported. - colcon build --symlink-install --packages-ignore skiros2_task + delib_build fi -source /delib_ws/install/setup.bash +source $ROS_WS/install/setup.bash # Execute the command passed into this entrypoint. exec "$@" From 004a3cf58bf5f7e94fd78e9a1b619283dc39899f Mon Sep 17 00:00:00 2001 From: David Conner Date: Mon, 9 Sep 2024 00:41:18 -0400 Subject: [PATCH 30/51] update flexbe_webui --- dependencies/FlexBE/flexbe_webui | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dependencies/FlexBE/flexbe_webui b/dependencies/FlexBE/flexbe_webui index 85a4b89..19f8c10 160000 --- a/dependencies/FlexBE/flexbe_webui +++ b/dependencies/FlexBE/flexbe_webui @@ -1 +1 @@ -Subproject commit 85a4b897b390c004fb4bb7093e61ca64001baa8b +Subproject commit 19f8c107c30a0826e41047ad1c7ffcce791dabbe From 0964684642c40d311f1046d804043ac065b81a7f Mon Sep 17 00:00:00 2001 From: David Conner Date: Tue, 10 Sep 2024 12:18:09 -0400 Subject: [PATCH 31/51] update flexbe_webui target --- dependencies/FlexBE/flexbe_webui | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dependencies/FlexBE/flexbe_webui b/dependencies/FlexBE/flexbe_webui index 19f8c10..cdabad4 160000 --- a/dependencies/FlexBE/flexbe_webui +++ b/dependencies/FlexBE/flexbe_webui @@ -1 +1 @@ -Subproject commit 19f8c107c30a0826e41047ad1c7ffcce791dabbe +Subproject commit cdabad4376b2bab4b74a9fdea645f8da138cd262 From eb9e2a55ece7b250d6d1cce78706e68e639cec40 Mon Sep 17 00:00:00 2001 From: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Date: Thu, 12 Sep 2024 10:21:38 -0400 Subject: [PATCH 32/51] Add nondefault navigation poses to dining room (#38) --- problems/delib_ws_worlds/worlds/world1.yaml | 6 ++++++ problems/delib_ws_worlds/worlds/world2.yaml | 6 ++++++ problems/delib_ws_worlds/worlds/world3.yaml | 6 ++++++ problems/delib_ws_worlds/worlds/world4.yaml | 6 ++++++ 4 files changed, 24 insertions(+) diff --git a/problems/delib_ws_worlds/worlds/world1.yaml b/problems/delib_ws_worlds/worlds/world1.yaml index f0e43cd..5d26be0 100644 --- a/problems/delib_ws_worlds/worlds/world1.yaml +++ b/problems/delib_ws_worlds/worlds/world1.yaml @@ -31,6 +31,12 @@ rooms: - [1.6, -1.0] - [1.6, 1.0] - [-1.6, 1.0] + # Add navigation poses since the centroid is blocked by the table. + nav_poses: + - [-1.0, 0.0, 0.0, 0.0] # left + - [1.0, 0.0, 0.0, 3.14] # right + - [0.0, 0.8, 0.0, -1.57] # above + - [0.0, -0.8, 0.0, 1.57] # below wall_width: 0.2 color: [0, 1, 0] diff --git a/problems/delib_ws_worlds/worlds/world2.yaml b/problems/delib_ws_worlds/worlds/world2.yaml index d70c29e..76ff03d 100644 --- a/problems/delib_ws_worlds/worlds/world2.yaml +++ b/problems/delib_ws_worlds/worlds/world2.yaml @@ -32,6 +32,12 @@ rooms: - [1.6, -1.0] - [1.6, 1.0] - [-1.6, 1.0] + # Add navigation poses since the centroid is blocked by the table. + nav_poses: + - [-1.0, 0.0, 0.0, 0.0] # left + - [1.0, 0.0, 0.0, 3.14] # right + - [0.0, 0.8, 0.0, -1.57] # above + - [0.0, -0.8, 0.0, 1.57] # below wall_width: 0.2 color: [0, 1, 0] diff --git a/problems/delib_ws_worlds/worlds/world3.yaml b/problems/delib_ws_worlds/worlds/world3.yaml index cb10646..c143613 100644 --- a/problems/delib_ws_worlds/worlds/world3.yaml +++ b/problems/delib_ws_worlds/worlds/world3.yaml @@ -46,6 +46,12 @@ rooms: - [1.6, -1.0] - [1.6, 1.0] - [-1.6, 1.0] + # Add navigation poses since the centroid is blocked by the table. + nav_poses: + - [-1.0, 0.0, 0.0, 0.0] # left + - [1.0, 0.0, 0.0, 3.14] # right + - [0.0, 0.8, 0.0, -1.57] # above + - [0.0, -0.8, 0.0, 1.57] # below wall_width: 0.2 color: [0, 1, 0] diff --git a/problems/delib_ws_worlds/worlds/world4.yaml b/problems/delib_ws_worlds/worlds/world4.yaml index 3c534a9..8360211 100644 --- a/problems/delib_ws_worlds/worlds/world4.yaml +++ b/problems/delib_ws_worlds/worlds/world4.yaml @@ -52,6 +52,12 @@ rooms: - [1.6, -1.0] - [1.6, 1.0] - [-1.6, 1.0] + # Add navigation poses since the centroid is blocked by the table. + nav_poses: + - [-1.0, 0.0, 0.0, 0.0] # left + - [1.0, 0.0, 0.0, 3.14] # right + - [0.0, 0.8, 0.0, -1.57] # above + - [0.0, -0.8, 0.0, 1.57] # below wall_width: 0.2 color: [0, 1, 0] From b0c4db88b78ae0a34db88e5ccbb7ea3d5a4a2d37 Mon Sep 17 00:00:00 2001 From: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Date: Thu, 12 Sep 2024 10:58:20 -0400 Subject: [PATCH 33/51] Tune RRT parameters (#26) --- problems/delib_ws_worlds/worlds/world1.yaml | 5 ++--- problems/delib_ws_worlds/worlds/world2.yaml | 5 ++--- problems/delib_ws_worlds/worlds/world3.yaml | 5 ++--- problems/delib_ws_worlds/worlds/world4.yaml | 5 ++--- 4 files changed, 8 insertions(+), 12 deletions(-) diff --git a/problems/delib_ws_worlds/worlds/world1.yaml b/problems/delib_ws_worlds/worlds/world1.yaml index 5d26be0..d5b20b0 100644 --- a/problems/delib_ws_worlds/worlds/world1.yaml +++ b/problems/delib_ws_worlds/worlds/world1.yaml @@ -17,10 +17,9 @@ robots: type: constant_velocity path_planner: type: rrt - rrt_star: true + bidirectional: true max_connection_dist: 1.0 - rewire_radius: 1.0 - max_time: 3.0 + max_time: 5.0 rooms: - name: dining diff --git a/problems/delib_ws_worlds/worlds/world2.yaml b/problems/delib_ws_worlds/worlds/world2.yaml index 76ff03d..c1633ed 100644 --- a/problems/delib_ws_worlds/worlds/world2.yaml +++ b/problems/delib_ws_worlds/worlds/world2.yaml @@ -18,10 +18,9 @@ robots: type: constant_velocity path_planner: type: rrt - rrt_star: true + bidirectional: true max_connection_dist: 1.0 - rewire_radius: 1.0 - max_time: 3.0 + max_time: 5.0 rooms: - name: dining diff --git a/problems/delib_ws_worlds/worlds/world3.yaml b/problems/delib_ws_worlds/worlds/world3.yaml index c143613..2c72947 100644 --- a/problems/delib_ws_worlds/worlds/world3.yaml +++ b/problems/delib_ws_worlds/worlds/world3.yaml @@ -18,10 +18,9 @@ robots: type: constant_velocity path_planner: type: rrt - rrt_star: true + bidirectional: true max_connection_dist: 1.0 - rewire_radius: 1.0 - max_time: 3.0 + max_time: 5.0 action_execution_options: navigate: success_probability: 0.9 diff --git a/problems/delib_ws_worlds/worlds/world4.yaml b/problems/delib_ws_worlds/worlds/world4.yaml index 8360211..5665794 100644 --- a/problems/delib_ws_worlds/worlds/world4.yaml +++ b/problems/delib_ws_worlds/worlds/world4.yaml @@ -18,10 +18,9 @@ robots: type: constant_velocity path_planner: type: rrt - rrt_star: true + bidirectional: true max_connection_dist: 1.0 - rewire_radius: 1.0 - max_time: 3.0 + max_time: 5.0 action_execution_options: navigate: success_probability: 0.9 From b15268f63958e892661f96025c012681e8fa7901 Mon Sep 17 00:00:00 2001 From: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Date: Sat, 14 Sep 2024 12:15:11 -0400 Subject: [PATCH 34/51] Install Groot2 inside container (#39) --- .docker/Dockerfile | 14 ++++++++++---- docker-compose.yaml | 2 ++ 2 files changed, 12 insertions(+), 4 deletions(-) diff --git a/.docker/Dockerfile b/.docker/Dockerfile index fef114b..9e296df 100644 --- a/.docker/Dockerfile +++ b/.docker/Dockerfile @@ -3,21 +3,27 @@ FROM ros:jazzy-ros-base AS roscon_delib_ws_2024 SHELL ["/bin/bash", "-o", "pipefail", "-c"] -# Install dependencies +# Install apt dependencies. ARG DEBIAN_FRONTEND=noninteractive RUN apt-get update && \ apt-get upgrade -y && \ apt-get install -y \ - apt-utils python3-pip python3-tk python3-wrapt python3-inflection \ + apt-utils curl fuse3 libfuse2 \ libegl1 libgl1-mesa-dev libglu1-mesa-dev '^libxcb.*-dev' libx11-xcb-dev \ libxi-dev libxkbcommon-dev libxkbcommon-x11-dev libxrender-dev \ - libnss3 libasound2t64 libxkbfile1 + libnss3 libasound2t64 libxkbfile1 \ + python3-pip python3-tk python3-wrapt python3-inflection # Create a ROS 2 workspace. RUN mkdir -p /delib_ws/src/ +WORKDIR /delib_ws # Install external dependencies. -WORKDIR /delib_ws +# Groot2 for BehaviorTree.CPP +RUN curl -o Groot2.AppImage https://s3.us-west-1.amazonaws.com/download.behaviortree.dev/groot2_linux_installer/Groot2-v1.6.1-x86_64.AppImage && \ + chmod a+x Groot2.AppImage && \ + groupadd fuse && \ + usermod -aG fuse root # Aggregated Python dependencies COPY python-requirements.txt /delib_ws RUN pip3 install --break-system-packages -r python-requirements.txt diff --git a/docker-compose.yaml b/docker-compose.yaml index 93b9dda..05967e7 100644 --- a/docker-compose.yaml +++ b/docker-compose.yaml @@ -27,6 +27,8 @@ services: # Networking and IPC for ROS 2 network_mode: host ipc: host + # Needed to run Groot inside the container + privileged: true environment: # Allows graphical programs in the container - DISPLAY=${DISPLAY} From 801281e3853a58f3ab2ff573c407ce9af90129bc Mon Sep 17 00:00:00 2001 From: Sebastian Castro Date: Sun, 15 Sep 2024 12:58:41 -0400 Subject: [PATCH 35/51] Update pyrobosim submodule --- dependencies/pyrobosim | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dependencies/pyrobosim b/dependencies/pyrobosim index 929e04b..4b27a3f 160000 --- a/dependencies/pyrobosim +++ b/dependencies/pyrobosim @@ -1 +1 @@ -Subproject commit 929e04b454096240013b692f34b927952a2d0b95 +Subproject commit 4b27a3ff3ed581205d55d856375aa103d832665c From fed0b507e4aaf7fe4e5eb41b9f655f2eb2af88cd Mon Sep 17 00:00:00 2001 From: Sebastian Castro Date: Mon, 16 Sep 2024 21:55:39 -0400 Subject: [PATCH 36/51] Update pyrobosim submodule to include reset UI button --- dependencies/pyrobosim | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dependencies/pyrobosim b/dependencies/pyrobosim index 4b27a3f..0c94fd2 160000 --- a/dependencies/pyrobosim +++ b/dependencies/pyrobosim @@ -1 +1 @@ -Subproject commit 4b27a3ff3ed581205d55d856375aa103d832665c +Subproject commit 0c94fd231acc233ddd128f433410fbb1257288cb From e5ddb08c290dd7004cc4f313b924f44ea2fdf81b Mon Sep 17 00:00:00 2001 From: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Date: Wed, 18 Sep 2024 16:11:58 -0400 Subject: [PATCH 37/51] Install gdb, nano, and RQT service caller in Docker image (#42) * Install gdb, nano, and rqt_service_caller in container * Remove NVIDIA env var in compose file --- .docker/Dockerfile | 5 +++-- docker-compose.yaml | 1 - 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/.docker/Dockerfile b/.docker/Dockerfile index 9e296df..16cad18 100644 --- a/.docker/Dockerfile +++ b/.docker/Dockerfile @@ -8,11 +8,12 @@ ARG DEBIAN_FRONTEND=noninteractive RUN apt-get update && \ apt-get upgrade -y && \ apt-get install -y \ - apt-utils curl fuse3 libfuse2 \ + apt-utils curl fuse3 libfuse2 gdb nano \ libegl1 libgl1-mesa-dev libglu1-mesa-dev '^libxcb.*-dev' libx11-xcb-dev \ libxi-dev libxkbcommon-dev libxkbcommon-x11-dev libxrender-dev \ libnss3 libasound2t64 libxkbfile1 \ - python3-pip python3-tk python3-wrapt python3-inflection + python3-pip python3-tk python3-wrapt python3-inflection \ + ros-jazzy-rqt-service-caller # Create a ROS 2 workspace. RUN mkdir -p /delib_ws/src/ diff --git a/docker-compose.yaml b/docker-compose.yaml index 05967e7..eb9f051 100644 --- a/docker-compose.yaml +++ b/docker-compose.yaml @@ -33,7 +33,6 @@ services: # Allows graphical programs in the container - DISPLAY=${DISPLAY} - QT_X11_NO_MITSHM=1 - - NVIDIA_DRIVER_CAPABILITIES=all # Enables FlexBE web UI to work as root inside the container - QTWEBENGINE_DISABLE_SANDBOX=1 volumes: From 4da3add3ee13715c55fc4930d6b7a1727ecde4d7 Mon Sep 17 00:00:00 2001 From: Matthias Mayr Date: Thu, 19 Sep 2024 21:29:59 +0200 Subject: [PATCH 38/51] Update: Newer commit hashes for SkiROS2 & skiros2_pyrobosim_lib (#43) --- dependencies/SkiROS2/skiros2 | 2 +- technologies/SkiROS2/skiros2_pyrobosim_lib | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/dependencies/SkiROS2/skiros2 b/dependencies/SkiROS2/skiros2 index 6b6cc38..b88b2c6 160000 --- a/dependencies/SkiROS2/skiros2 +++ b/dependencies/SkiROS2/skiros2 @@ -1 +1 @@ -Subproject commit 6b6cc38df79a99e16ddc2f0b8e9f49fadaa9f284 +Subproject commit b88b2c6dd4da51e27d44cb14b53da240be3487c1 diff --git a/technologies/SkiROS2/skiros2_pyrobosim_lib b/technologies/SkiROS2/skiros2_pyrobosim_lib index f8eed45..e28d574 160000 --- a/technologies/SkiROS2/skiros2_pyrobosim_lib +++ b/technologies/SkiROS2/skiros2_pyrobosim_lib @@ -1 +1 @@ -Subproject commit f8eed45e02ef93c0214f3dd2791f7d7b6b4a1e74 +Subproject commit e28d574ee1de9f8bc865bbcf010deacbaefce53a From f335fe80cb203acc4a4497706a6bc02f8af00408 Mon Sep 17 00:00:00 2001 From: Christian Henkel <6976069+ct2034@users.noreply.github.com> Date: Mon, 23 Sep 2024 17:39:33 +0200 Subject: [PATCH 39/51] Adding convince toolchain to docker and a small example to technologies. (#44) * adding as2fm as submodule * adding bt_tools * technology readme * convince example --------- Signed-off-by: Christian Henkel Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Co-authored-by: Sebastian Castro --- .docker/Dockerfile | 13 +++ .gitmodules | 6 + dependencies/convince/AS2FM | 1 + dependencies/convince/bt_tools | 1 + technologies/README.md | 1 + technologies/convince/README.md | 66 +++++++++++ .../example_w_bt/battery_drainer.scxml | 30 +++++ .../example_w_bt/battery_manager.scxml | 26 +++++ .../example_w_bt/battery_properties.jani | 107 ++++++++++++++++++ technologies/convince/example_w_bt/bt.xml | 8 ++ .../example_w_bt/bt_topic_action.scxml | 22 ++++ .../example_w_bt/bt_topic_condition.scxml | 31 +++++ technologies/convince/example_w_bt/main.xml | 20 ++++ 13 files changed, 332 insertions(+) create mode 160000 dependencies/convince/AS2FM create mode 160000 dependencies/convince/bt_tools create mode 100644 technologies/convince/README.md create mode 100644 technologies/convince/example_w_bt/battery_drainer.scxml create mode 100644 technologies/convince/example_w_bt/battery_manager.scxml create mode 100644 technologies/convince/example_w_bt/battery_properties.jani create mode 100644 technologies/convince/example_w_bt/bt.xml create mode 100644 technologies/convince/example_w_bt/bt_topic_action.scxml create mode 100644 technologies/convince/example_w_bt/bt_topic_condition.scxml create mode 100644 technologies/convince/example_w_bt/main.xml diff --git a/.docker/Dockerfile b/.docker/Dockerfile index 16cad18..767baf2 100644 --- a/.docker/Dockerfile +++ b/.docker/Dockerfile @@ -33,6 +33,19 @@ RUN source /opt/ros/jazzy/setup.bash && \ rosdep install --from-paths src -y --ignore-src # SkiROS2 dependencies RUN src/dependencies/SkiROS2/skiros2/skiros2/scripts/install_fd_task_planner.sh +# Convince toolchain +# (smc_storm) +RUN curl -O -L https://github.com/convince-project/smc_storm/releases/download/0.0.3/smc_storm_executable.tar.gz && \ + tar -xzf smc_storm_executable.tar.gz && \ + ./install.sh --install-dependencies && \ + ln -s /delib_ws/bin/smc_storm /usr/local/bin/smc_storm +# (AS2FM) +RUN cd src/dependencies/convince/AS2FM && \ + pip3 install --break-system-packages as2fm_common/. && \ + pip3 install --break-system-packages jani_generator/. && \ + pip3 install --break-system-packages scxml_converter/. && \ + pip3 uninstall -y --break-system-packages js2py && \ + pip3 install --break-system-packages git+https://github.com/felixonmars/Js2Py.git@py3.12 # Temporary fix for js2py # Remove MatPlotLib display warnings. RUN mkdir /tmp/runtime-root diff --git a/.gitmodules b/.gitmodules index 89b07c2..c39cae7 100644 --- a/.gitmodules +++ b/.gitmodules @@ -34,3 +34,9 @@ path = dependencies/BehaviorTree.ROS2 url = https://github.com/BehaviorTree/BehaviorTree.ROS2.git branch = humble +[submodule "dependencies/convince/AS2FM"] + path = dependencies/convince/AS2FM + url = https://github.com/convince-project/AS2FM.git +[submodule "dependencies/convince/bt_tools"] + path = dependencies/convince/bt_tools + url = https://github.com/boschresearch/bt_tools.git diff --git a/dependencies/convince/AS2FM b/dependencies/convince/AS2FM new file mode 160000 index 0000000..d389259 --- /dev/null +++ b/dependencies/convince/AS2FM @@ -0,0 +1 @@ +Subproject commit d389259f65a37f58afafe6116f759cd60e0f0497 diff --git a/dependencies/convince/bt_tools b/dependencies/convince/bt_tools new file mode 160000 index 0000000..c2b7d66 --- /dev/null +++ b/dependencies/convince/bt_tools @@ -0,0 +1 @@ +Subproject commit c2b7d66d2c80be45a2028c278987b94f3fa2943d diff --git a/technologies/README.md b/technologies/README.md index e3af149..a1bb1ab 100644 --- a/technologies/README.md +++ b/technologies/README.md @@ -7,3 +7,4 @@ The technologies in this workshop include: * [ros_bt_py](https://github.com/fzi-forschungszentrum-informatik/ros2_ros_bt_py) - A Python based library for behavior trees. * [FlexBE](https://github.com/FlexBE) - A Python based library for hierarchical finite-state machines. * [SkiROS2](https://github.com/RVMI/skiros2) - A Python based library for implementing abstract skill interfaces and performing task planning. +* [AS2FM by CONVINCE](https://github.com/convince-project/AS2FM) - A tool for model checking ROS 2 based systems. diff --git a/technologies/convince/README.md b/technologies/convince/README.md new file mode 100644 index 0000000..66c75ff --- /dev/null +++ b/technologies/convince/README.md @@ -0,0 +1,66 @@ +# AS2FM by CONVINCE + +This is an introduction to the tool [AS2FM (Autonomous Systems to Formal Models)](https://convince-project.github.io/AS2FM/index.html) that is part of the [CONVINCE toolchain](https://convince-project.github.io/overview/). +While there is also a [full documentation](https://convince-project.github.io/AS2FM/index.html), this is a short introductory example. + +## Introduction to example + +The folder `technologies/convince/example_w_bt` contains a model of a simplistic system that consist of a battery that is drained at a fixed rate and the logic to charge it when it is low. + +In the folder you will find the following files: + +- `main.xml` - The main file referencing the other files and defining some global system parameters. +- __`battery_drainer.scxml`__ - The state machine that models the battery which is drained. +- __`battery_manager.scxml`__ - Model of the manager that will evaluate the battery level. +- __`bt.xml`__ - The behavior tree that implements the charging logic. +- __`bt_topic_action.scxml`__ - The model of the action plugin that allows the behavior tree to trigger ROS actions. +- __`bt_topic_condition.scxml`__ - The model of the condition plugin that allows the behavior tree to check ROS topics. +- __`battery_properties.jani`__ - The file defining the properties to be checked by the model checker: + - `battery_depleted` - The battery is eventually depleted. + - `battery_below_20` - The battery is eventually below 20%. + - `battery_alarm_on` - The alarm is eventually on. + - `battery_charged` - The battery will always eventually be at 100% again. + +## Prerequisites + +This example expects btlib to be built. +This should normally be the case in the docker image. If not, run this: + +```bash +cd $ROS_WS +colcon build --symlink-install --packages-select btlib +source ./install/setup.bash +``` + +## Running the example + +To run the example, navigate to the `example_w_bt` folder: + +```bash +cd $ROS_WS/src/technologies/convince/example_w_bt +``` + +First, you need to translate all of the files mentioned above into a single JANI model file: + +```bash +scxml_to_jani main.xml +``` + +Note that there is now additionally a file `main.jani` in the folder. + +Then, you can run the model checker on the generated JANI file, specifying the property to check: + +```bash +smc_storm --model main.jani --properties-names battery_charged +``` + +## Expected output + +This will check the property `battery_charged` and output the result of the model checking which should look something like this: + +```bash +CONVINCE Statistical Model Checker +Checking model: main.jani +Property "battery_charged": Pmin=? [true Usteps>=100 ((topic_level_msg.data = 100) & topic_level_msg.valid)]; +Result: 1 +``` diff --git a/technologies/convince/example_w_bt/battery_drainer.scxml b/technologies/convince/example_w_bt/battery_drainer.scxml new file mode 100644 index 0000000..5e17f31 --- /dev/null +++ b/technologies/convince/example_w_bt/battery_drainer.scxml @@ -0,0 +1,30 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/technologies/convince/example_w_bt/battery_manager.scxml b/technologies/convince/example_w_bt/battery_manager.scxml new file mode 100644 index 0000000..ee8b829 --- /dev/null +++ b/technologies/convince/example_w_bt/battery_manager.scxml @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/technologies/convince/example_w_bt/battery_properties.jani b/technologies/convince/example_w_bt/battery_properties.jani new file mode 100644 index 0000000..8f7c839 --- /dev/null +++ b/technologies/convince/example_w_bt/battery_properties.jani @@ -0,0 +1,107 @@ +{ + "properties": [ + { + "name": "battery_depleted", + "expression": { + "op": "filter", + "fun": "values", + "values": { + "op": "Pmin", + "exp": { + "left": true, + "op": "U", + "right": { + "left": { + "op": "≤", + "left": "topic_level_msg.data", + "right": 0 + }, + "op": "∧", + "right": "topic_level_msg.valid" + } + } + }, + "states": { + "op": "initial" + } + } + }, + { + "name": "battery_below_20", + "expression": { + "op": "filter", + "fun": "values", + "values": { + "op": "Pmin", + "exp": { + "left": true, + "op": "U", + "right": { + "left": { + "op": "<", + "left": "topic_level_msg.data", + "right": 20 + }, + "op": "∧", + "right": "topic_level_msg.valid" + } + } + }, + "states": { + "op": "initial" + } + } + }, + { + "name": "battery_alarm_on", + "expression": { + "op": "filter", + "fun": "values", + "values": { + "op": "Pmin", + "exp": { + "left": true, + "op": "U", + "right": { + "op": "∧", + "left": "topic_alarm_msg.data", + "right": "topic_charge_msg.valid" + } + } + }, + "states": { + "op": "initial" + } + } + }, + { + "name": "battery_charged", + "expression": { + "op": "filter", + "fun": "values", + "values": { + "op": "Pmin", + "exp": { + "step-bounds": { + "lower": 100 + }, + "left": true, + "op": "U", + "right": { + "left": { + "op": "=", + "left": "topic_level_msg.data", + "right": 100 + }, + "op": "∧", + "right": "topic_level_msg.valid" + } + } + }, + "states": { + "op": "initial" + } + } + } + ] +} diff --git a/technologies/convince/example_w_bt/bt.xml b/technologies/convince/example_w_bt/bt.xml new file mode 100644 index 0000000..e5eadfb --- /dev/null +++ b/technologies/convince/example_w_bt/bt.xml @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/technologies/convince/example_w_bt/bt_topic_action.scxml b/technologies/convince/example_w_bt/bt_topic_action.scxml new file mode 100644 index 0000000..8fd0168 --- /dev/null +++ b/technologies/convince/example_w_bt/bt_topic_action.scxml @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + diff --git a/technologies/convince/example_w_bt/bt_topic_condition.scxml b/technologies/convince/example_w_bt/bt_topic_condition.scxml new file mode 100644 index 0000000..58ba0da --- /dev/null +++ b/technologies/convince/example_w_bt/bt_topic_condition.scxml @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/technologies/convince/example_w_bt/main.xml b/technologies/convince/example_w_bt/main.xml new file mode 100644 index 0000000..cf6888f --- /dev/null +++ b/technologies/convince/example_w_bt/main.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + From a83aabd52306d6a80842962b1fef9bdf77aa096f Mon Sep 17 00:00:00 2001 From: Sebastian Castro Date: Mon, 23 Sep 2024 20:32:00 -0400 Subject: [PATCH 40/51] Temporary fix for conflicting matplotlib apt and pip installs --- .docker/Dockerfile | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.docker/Dockerfile b/.docker/Dockerfile index 767baf2..9bb4daa 100644 --- a/.docker/Dockerfile +++ b/.docker/Dockerfile @@ -45,7 +45,8 @@ RUN cd src/dependencies/convince/AS2FM && \ pip3 install --break-system-packages jani_generator/. && \ pip3 install --break-system-packages scxml_converter/. && \ pip3 uninstall -y --break-system-packages js2py && \ - pip3 install --break-system-packages git+https://github.com/felixonmars/Js2Py.git@py3.12 # Temporary fix for js2py + pip3 install --break-system-packages git+https://github.com/felixonmars/Js2Py.git@py3.12 # Temporary fix for js2py && \ + pip3 uninstall -y --break-system-packages matplotlib # Temporary fix for MatPlotLib conflict # Remove MatPlotLib display warnings. RUN mkdir /tmp/runtime-root From 812d674dde24be74e5854a956130c960c534923c Mon Sep 17 00:00:00 2001 From: Sebastian Castro Date: Wed, 25 Sep 2024 23:37:29 -0400 Subject: [PATCH 41/51] Update PyRoboSim --- dependencies/pyrobosim | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dependencies/pyrobosim b/dependencies/pyrobosim index 0c94fd2..18c85e0 160000 --- a/dependencies/pyrobosim +++ b/dependencies/pyrobosim @@ -1 +1 @@ -Subproject commit 0c94fd231acc233ddd128f433410fbb1257288cb +Subproject commit 18c85e001add30d6bc9836b3ffa53d511cf863a1 From 47991f84ec2c3047f53e631a5357b38da2b086d0 Mon Sep 17 00:00:00 2001 From: dcconner Date: Fri, 27 Sep 2024 19:59:07 -0400 Subject: [PATCH 42/51] Flexbe update (#49) Add additional behaviors, update README, and provide additional state implementations --- .docker/entrypoint.sh | 7 + dependencies/FlexBE/flexbe_behavior_engine | 2 +- dependencies/FlexBE/flexbe_webui | 2 +- dependencies/pyrobosim | 2 +- problems/delib_ws_worlds/worlds/world4.yaml | 2 +- technologies/FlexBE/.flake8 | 8 + technologies/FlexBE/README.md | 79 +++++- .../manifest/delib_ws_p2_sm.xml | 2 +- .../manifest/detectselect.xml | 4 +- .../manifest/go_beh.xml | 24 ++ .../manifest/patrol.xml | 19 ++ .../manifest/patrolcharge.xml | 20 ++ .../manifest/test_pick_place.xml | 6 +- .../manifest/through_door.xml | 18 ++ .../manifest/traverse.xml | 26 ++ .../pyrobosim_flexbe_behaviors/__init__.py | 1 + .../delib_ws_p2_sm.py | 2 +- .../delib_ws_p2_sm_sm.py | 6 +- .../detectselect_sm.py | 4 +- .../pyrobosim_flexbe_behaviors/go_beh_sm.py | 153 ++++++++++++ .../pyrobosim_flexbe_behaviors/patrol_sm.py | 190 +++++++++++++++ .../patrolcharge_sm.py | 192 +++++++++++++++ .../test_navigate_sm.py | 56 ++--- .../test_pick_place_sm.py | 30 +-- .../test_plan_path_sm.py | 18 +- .../through_door_sm.py | 224 ++++++++++++++++++ .../pyrobosim_flexbe_behaviors/traverse_sm.py | 150 ++++++++++++ .../pyrobosim_flexbe_behaviors/setup.py | 9 +- .../pyrobosim_flexbe_states/package.xml | 1 + .../pyrobosim_flexbe_states/__init__.py | 1 + .../check_door_state.py | 206 ++++++++++++++++ .../detect_local_objects_state.py | 51 ++-- .../detect_objects_state.py | 40 +++- .../door_action_state.py | 30 ++- .../follow_path_state.py | 62 +++-- .../monitor_battery_state.py | 157 ++++++++++++ .../navigate_action_state.py | 35 ++- .../next_room_state.py | 187 +++++++++++++++ .../pick_action_state.py | 33 ++- .../place_action_state.py | 38 ++- .../plan_path_state.py | 73 ++++-- .../FlexBE/pyrobosim_flexbe_states/setup.py | 20 +- .../tests/launch_test.py | 27 ++- .../tests/run_colcon_test.py | 6 +- .../pyrobosim_flexbe_utilities/package.xml | 34 +++ .../pyrobosim_flexbe_utilities/__init__.py | 1 + .../world_configuration.py | 202 ++++++++++++++++ .../resource/pyrobosim_flexbe_utilities | 0 .../pyrobosim_flexbe_utilities/setup.cfg | 4 + .../pyrobosim_flexbe_utilities/setup.py | 30 +++ 50 files changed, 2275 insertions(+), 219 deletions(-) create mode 100644 technologies/FlexBE/.flake8 create mode 100644 technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/go_beh.xml create mode 100644 technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/patrol.xml create mode 100644 technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/patrolcharge.xml create mode 100644 technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/through_door.xml create mode 100644 technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/traverse.xml create mode 100644 technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/go_beh_sm.py create mode 100644 technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/patrol_sm.py create mode 100644 technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/patrolcharge_sm.py create mode 100644 technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/through_door_sm.py create mode 100644 technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/traverse_sm.py create mode 100644 technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/check_door_state.py create mode 100644 technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/monitor_battery_state.py create mode 100644 technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/next_room_state.py create mode 100644 technologies/FlexBE/pyrobosim_flexbe_utilities/package.xml create mode 100644 technologies/FlexBE/pyrobosim_flexbe_utilities/pyrobosim_flexbe_utilities/__init__.py create mode 100644 technologies/FlexBE/pyrobosim_flexbe_utilities/pyrobosim_flexbe_utilities/world_configuration.py create mode 100644 technologies/FlexBE/pyrobosim_flexbe_utilities/resource/pyrobosim_flexbe_utilities create mode 100644 technologies/FlexBE/pyrobosim_flexbe_utilities/setup.cfg create mode 100644 technologies/FlexBE/pyrobosim_flexbe_utilities/setup.py diff --git a/.docker/entrypoint.sh b/.docker/entrypoint.sh index 8d3998f..6ee9f80 100755 --- a/.docker/entrypoint.sh +++ b/.docker/entrypoint.sh @@ -56,6 +56,13 @@ function delib_clean () { cd $cwd } +# Use this to switch to software rendering to avoid +# conflicts with GPU and docker +function qt_soft_render() { + export QT_QUICK_BACKEND=software + echo "Using $QT_QUICK_BACKEND for QT rendering" +} + # Automatic build when entering the container if [ ! -f $ROS_WS/install/setup.bash ] then diff --git a/dependencies/FlexBE/flexbe_behavior_engine b/dependencies/FlexBE/flexbe_behavior_engine index f176c03..e5fa935 160000 --- a/dependencies/FlexBE/flexbe_behavior_engine +++ b/dependencies/FlexBE/flexbe_behavior_engine @@ -1 +1 @@ -Subproject commit f176c0305f54643ac5123841dadae66ebbf4872e +Subproject commit e5fa9356de46ec60c50e3b43acc9245f92e2135a diff --git a/dependencies/FlexBE/flexbe_webui b/dependencies/FlexBE/flexbe_webui index cdabad4..453b7a7 160000 --- a/dependencies/FlexBE/flexbe_webui +++ b/dependencies/FlexBE/flexbe_webui @@ -1 +1 @@ -Subproject commit cdabad4376b2bab4b74a9fdea645f8da138cd262 +Subproject commit 453b7a7b9c44d138796e467f650de07264c771c0 diff --git a/dependencies/pyrobosim b/dependencies/pyrobosim index 18c85e0..9157eed 160000 --- a/dependencies/pyrobosim +++ b/dependencies/pyrobosim @@ -1 +1 @@ -Subproject commit 18c85e001add30d6bc9836b3ffa53d511cf863a1 +Subproject commit 9157eed1a3668a6b0f0e54116790cc2d38300285 diff --git a/problems/delib_ws_worlds/worlds/world4.yaml b/problems/delib_ws_worlds/worlds/world4.yaml index 5665794..80f5214 100644 --- a/problems/delib_ws_worlds/worlds/world4.yaml +++ b/problems/delib_ws_worlds/worlds/world4.yaml @@ -206,7 +206,7 @@ locations: parent: kitchen category: storage pose: [-3.65, -1.5, 0.0, 1.57] - is_open: true + is_open: false - name: bin parent: office diff --git a/technologies/FlexBE/.flake8 b/technologies/FlexBE/.flake8 new file mode 100644 index 0000000..b93ef7c --- /dev/null +++ b/technologies/FlexBE/.flake8 @@ -0,0 +1,8 @@ +[flake8] +exclude = .git +max-line-length = 130 +ignore = W503, D100, D105, D107, E203, F401 +# Ignore docstring requirements for public modules, magic methods, and __init__ functions. +# For W503 - https://www.flake8rules.com/rules/W503.html and https://peps.python.org/pep-0008/#should-a-line-break-before-or-after-a-binary-operator +# For E203 the state machine writer starts line with , +# F401 state machine writer adds imports by default diff --git a/technologies/FlexBE/README.md b/technologies/FlexBE/README.md index 0479136..de84c4e 100644 --- a/technologies/FlexBE/README.md +++ b/technologies/FlexBE/README.md @@ -1,7 +1,5 @@ # FlexBE States and Behaviors for ROSCon 2024 Deliberation Technologies Workshop -> NOTE: This is a very preliminary version - # Installation and Setup @@ -11,7 +9,9 @@ To build locally at home see the [installation instructions](docs/installation.m ## Usage -We provide a few behaviors, for now use the `delib_ws_p2_sm` behavior. +We provide a few behaviors as described below. + +This quick start uses the `delib_ws_p2_sm` behavior. This includes a statemachine with on nested sub-statemachine to allow selection of a door to open, and a nested behavior called `DetectSelect` that plans path to location, retrieves a user selected object, and places at target location. @@ -31,6 +31,11 @@ To run start the following in separate terminals * Note: It is best to give the OCS software several seconds to start and load behaviors before launching the UI Wait on the `Begin behavior mirror processing ...` message in terminal +> Note: On some systems, the docker and GPU interactions interfere with UI rendering. +> `export QT_QUICK_BACKEND=software` in terminal or use the aliased helper function +> `qt_soft_render()` before launching the `webui_client` + + * `clear; ros2 run flexbe_input input_action_server` * This launches an action server that will pop up a simple UI that will allow the operator input data on request * In this demonstration, it is used to select from among the detected objects @@ -55,10 +60,68 @@ To run start the following in separate terminals > WARNING: An operator can preempt a state, so wait for the state to finish and request the outcome by highlighting the transition. - > NOTE: Much more to follow. + Alternatively you can try : + * `clear; ros2 run delib_ws_worlds run --ros-args -p problem_number:=4` + + And load `PatrolCharge` to move about all rooms and recharge the battery when level drops below 30% charge. + This uses a `ConcurrencyContainer` and a `PriorityContainer` to preempt the `Patrol` behavior and recharge the battery. + + +There are a number of demonstration behaviors created to demonstrate various state implementations + +### Provided State Implementations + +The `flexbe_states` package in the `dependencies/FlexBE/flexbe_behavior_engine` submodule provides a number of generic states. + +Specifically we demonstrate: +* `LogState` - print message to terminal (both console and UI) +* `LogKeyState` - print message containing user data value at specified key +* `OperatorDecisionState` - allow operator to select among specified outcomes; in full autonomy the "suggested" outcome is selected. +* `SelectionState` - uses `input_action_server` to allow user to select among given data +* `InputState` - uses `input_action_server` to allow user to input simple data such as string, number, or list of numbers + +The `pyrobosim_flexbe_states` package under `technologies/FlexBE` includes: +* `check_door_state.py` - Uses pyrobosim `RequestWorldState` service to check door status using non-blocking call +* `detect_local_objects_state.py` - Invokes a pyrobosim `ExecuteTaskAction` to `detect` objects, then uses `RequestWorldState` +* `detect_objects_state.py` - Invokes the pyrobosim `DetectObjects` action to detect objects, then adds to user data +* `door_action_state.py` - Invoke `ExecuteTaskAction` to `open` or `close` an openable (including doors, dumpster, pantry, fridge, ...) +* `follow_path_state.py` - Follow current path passed as user data into state +* `monitor_battery_state.py` - Monitor battery state and return outcome if either low or high level detected (blocks, use in concurrent state) +* `navigate_action_state.py` - Navigation action that combines planning and following +* `next_room_state.py` - Does simple planning to determine adjacent room based on connected rooms +* `pick_action_state.py` - Does pick action at current location +* `place_action_state.py` - Does place action at current location +* `plan_path_state.py` - Request plan from current location to target location + +The above states have extra logging information that is shown in the onboard behavior terminal. Each state transition `on_start`, `on_enter`, `on_exit`, `on_pause`, `on_resume`, and `on_stop` are logged. This is not recommended in regular states, but is done here for educational purposes. + +### Basic Demonstration Behaviors + +In order of increasing complexity + +Use `clear; ros2 run delib_ws_worlds run --ros-args -p problem_number:=1` + +* `test navigate` - Test navigation state +* `Test Plan Path` - Simple plan then follow behavior with userdata +* `test pick place` - Navigation with pick and place using behavior parameters +* `DetectSelect` - Move, detect objects, select using `input_action_server`, move and place +* `Go Beh` - Go to target location + +Use `clear; ros2 run delib_ws_worlds run --ros-args -p problem_number:=1` + +* `delib_ws_p2` - use behavior parameters plan to door, open door, then use `DetectSelect` embedded behavior +* `delib_ws_p2_sm` - use hierarchical state machine to select door, open, then use `DetectSelect` embedded behavior +* `Through Door` - Travel through doorway opening if necessary +* `Traverse` - Go to specified target location, opening intermediate doors if necessary using `Go Beh` and `Through Door` + * This incorporates a simple planning `NextRoomState` to determine the next adjacent room based on world structure + +* `Patrol` - traverses each room in particular order using `Traverse` behavior + * You may want to use low autonomy and confirm reasonable paths and guide to open doors initially before switching to full autonomy + + Use `clear; ros2 run delib_ws_worlds run --ros-args -p problem_number:=4` -## To dos + * `PatrolCharge` - This uses a `Concurrency` container that includes the `Patrol` behavior and a state machine that monitors battery level. + * On low battery level (set to 30%), a `PriorityContainer` invokes the `Traverse` behavior to go to charging dock. + * This pauses the `Patrol` behavior until finished charging. On resuming, any active planning or follow states will return `failed` and restart the `Patrol` behavior. -- [ ] Battery monitor state and concurrency example -- [ ] Define (sub-)behaviors for workshop tasks -- [ ] Write up more detail usage instructions +You can build on these behavior demonstrations to solve the workshop tasks. diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/delib_ws_p2_sm.xml b/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/delib_ws_p2_sm.xml index fdd1e66..c34f971 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/delib_ws_p2_sm.xml +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/delib_ws_p2_sm.xml @@ -8,7 +8,7 @@ Sat Aug 31 2024 Behavior demonstrating p2 - travel, open door, go to table, pick object, - transport, and place + transport, and place using embedded state machine and operator selection. diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/detectselect.xml b/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/detectselect.xml index 56dc752..ff0be08 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/detectselect.xml +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/detectselect.xml @@ -17,9 +17,9 @@ - + - + diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/go_beh.xml b/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/go_beh.xml new file mode 100644 index 0000000..a7e1504 --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/go_beh.xml @@ -0,0 +1,24 @@ + + + + + + pyrobosim, delib_ws + Conner + Sun Sep 08 2024 + + Go to target behavior + + + + + + + + + + + + + + diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/patrol.xml b/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/patrol.xml new file mode 100644 index 0000000..30a8db9 --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/patrol.xml @@ -0,0 +1,19 @@ + + + + + + pyrobosim, patrol + Conner + Mon Sep 23 2024 + + Patrol rooms without concern for battery + + + + + + + + + diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/patrolcharge.xml b/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/patrolcharge.xml new file mode 100644 index 0000000..74660a8 --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/patrolcharge.xml @@ -0,0 +1,20 @@ + + + + + + pyrobosim, battery + Conner + Mon Sep 23 2024 + + Patrol and recharge as necessary + + + + + + + + + + diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/test_pick_place.xml b/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/test_pick_place.xml index c625d63..e6664ea 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/test_pick_place.xml +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/test_pick_place.xml @@ -16,11 +16,11 @@ - + - + - + diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/through_door.xml b/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/through_door.xml new file mode 100644 index 0000000..189c5f4 --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/through_door.xml @@ -0,0 +1,18 @@ + + + + + + pyrobosim + conner + Sun Sep 08 2024 + + Go through a door, opening if required + + + + + + + + diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/traverse.xml b/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/traverse.xml new file mode 100644 index 0000000..a2ffd25 --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/traverse.xml @@ -0,0 +1,26 @@ + + + + + + pyrobosim, delib_ws + conner + Sun Sep 08 2024 + + Traverse the world opening doors as required + + + + + + + + + + + + + + + + diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/__init__.py b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/__init__.py index e69de29..5dae5d3 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/__init__.py +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/__init__.py @@ -0,0 +1 @@ +"""pyrobosim_flexbe_behaviors module.""" diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/delib_ws_p2_sm.py b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/delib_ws_p2_sm.py index 6047fbf..5339aae 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/delib_ws_p2_sm.py +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/delib_ws_p2_sm.py @@ -54,7 +54,7 @@ # [/MANUAL_IMPORT] -class delibwsp2SM(Behavior): +class delib_ws_p2SM(Behavior): """ Define delib_ws_p2. diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/delib_ws_p2_sm_sm.py b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/delib_ws_p2_sm_sm.py index e860ea4..6c39664 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/delib_ws_p2_sm_sm.py +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/delib_ws_p2_sm_sm.py @@ -26,7 +26,7 @@ Define delib_ws_p2_sm. Behavior demonstrating p2 - travel, open door, go to table, pick object, -transport, and place +transport, and place using embedded state machine and operator selection. Created on Sat Aug 31 2024 @author: David Conner @@ -55,12 +55,12 @@ # [/MANUAL_IMPORT] -class delibwsp2smSM(Behavior): +class delib_ws_p2_smSM(Behavior): """ Define delib_ws_p2_sm. Behavior demonstrating p2 - travel, open door, go to table, pick object, - transport, and place + transport, and place using embedded state machine and operator selection. """ def __init__(self, node): diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/detectselect_sm.py b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/detectselect_sm.py index 2eedf2a..4a248f4 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/detectselect_sm.py +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/detectselect_sm.py @@ -70,8 +70,8 @@ def __init__(self, node): self.name = 'DetectSelect' # parameters of this behavior - self.add_parameter('move_location', 'counter0_right') - self.add_parameter('place_location', 'desk0') + self.add_parameter('move_location', 'pantry') + self.add_parameter('place_location', 'desk') # Initialize ROS node information initialize_flexbe_core(node) diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/go_beh_sm.py b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/go_beh_sm.py new file mode 100644 index 0000000..312a39d --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/go_beh_sm.py @@ -0,0 +1,153 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +# Copyright 2024 Conner +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +########################################################### +# WARNING: Generated code! # +# ************************** # +# Manual changes may get lost if file is generated again. # +# Only code inside the [MANUAL] tags will be kept. # +########################################################### + +""" +Define Go Beh. + +Go to target behavior + +Created on Sun Sep 08 2024 +@author: Conner +""" + + +from flexbe_core import Autonomy +from flexbe_core import Behavior +from flexbe_core import ConcurrencyContainer +from flexbe_core import Logger +from flexbe_core import OperatableStateMachine +from flexbe_core import PriorityContainer +from flexbe_core import initialize_flexbe_core +from flexbe_states.log_state import LogState +from flexbe_states.operator_decision_state import OperatorDecisionState +from pyrobosim_flexbe_states.follow_path_state import FollowPathState +from pyrobosim_flexbe_states.plan_path_state import PlanPathState + +# Additional imports can be added inside the following tags +# [MANUAL_IMPORT] + + +# [/MANUAL_IMPORT] + + +class GoBehSM(Behavior): + """ + Define Go Beh. + + Go to target behavior + """ + + def __init__(self, node): + super().__init__() + self.name = 'Go Beh' + + # parameters of this behavior + self.add_parameter('target', 'desk') + + # Initialize ROS node information + initialize_flexbe_core(node) + + # references to used behaviors + + # Additional initialization code can be added inside the following tags + # [MANUAL_INIT] + + + # [/MANUAL_INIT] + + # Behavior comments: + + def create(self): + """Create state machine.""" + # Root state machine + # x:1061 y:86, x:1083 y:307 + _state_machine = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['goal'], output_keys=['msg']) + _state_machine.userdata.goal = self.target + _state_machine.userdata.msg = '' + + # Additional creation code can be added inside the following tags + # [MANUAL_CREATE] + + + # [/MANUAL_CREATE] + + with _state_machine: + # x:117 y:74 + OperatableStateMachine.add('PlanPath', + PlanPathState(action_topic='robot/plan_path', + timeout=6.0), + transitions={'done': 'UsePlan' # 310 88 -1 -1 -1 -1 + , 'failed': 'failed' # 274 287 154 127 -1 -1 + }, + autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off}, + remapping={'goal': 'goal', 'msg': 'msg', 'path': 'path'}) + + # x:696 y:190 + OperatableStateMachine.add('AskRetry', + OperatorDecisionState(outcomes=['retry', 'fail'], + hint='retry', + suggestion='retry'), + transitions={'retry': 'LogReplan' # 441 230 -1 -1 362 223 + , 'fail': 'failed' # 873 278 778 243 -1 -1 + }, + autonomy={'retry': Autonomy.High, 'fail': Autonomy.Full}) + + # x:584 y:84 + OperatableStateMachine.add('FollowPath', + FollowPathState(action_topic='robot/follow_path', + server_timeout=2.0), + transitions={'done': 'finished' # 900 101 -1 -1 -1 -1 + , 'failed': 'AskRetry' # 789 151 720 114 -1 -1 + }, + autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off}, + remapping={'path': 'path', 'msg': 'msg'}) + + # x:278 y:194 + OperatableStateMachine.add('LogReplan', + LogState(text="Replan request", + severity=Logger.REPORT_INFO), + transitions={'done': 'PlanPath' # 220 193 -1 -1 -1 -1 + }, + autonomy={'done': Autonomy.Off}) + + # x:345 y:71 + OperatableStateMachine.add('UsePlan', + OperatorDecisionState(outcomes=['use', 'replan', 'fail'], + hint='use', + suggestion='use'), + transitions={'use': 'FollowPath' # 538 99 -1 -1 -1 -1 + , 'replan': 'LogReplan' # 420 189 419 124 -1 -1 + , 'fail': 'failed' # 569 256 -1 -1 -1 -1 + }, + autonomy={'use': Autonomy.Low, + 'replan': Autonomy.Full, + 'fail': Autonomy.Full}) + + return _state_machine + + # Private functions can be added inside the following tags + # [MANUAL_FUNC] + + + # [/MANUAL_FUNC] diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/patrol_sm.py b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/patrol_sm.py new file mode 100644 index 0000000..505a49f --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/patrol_sm.py @@ -0,0 +1,190 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +# Copyright 2024 Conner +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +########################################################### +# WARNING: Generated code! # +# ************************** # +# Manual changes may get lost if file is generated again. # +# Only code inside the [MANUAL] tags will be kept. # +########################################################### + +""" +Define Patrol. + +Patrol rooms without concern for battery + +Created on Mon Sep 23 2024 +@author: Conner +""" + + +from flexbe_core import Autonomy +from flexbe_core import Behavior +from flexbe_core import ConcurrencyContainer +from flexbe_core import Logger +from flexbe_core import OperatableStateMachine +from flexbe_core import PriorityContainer +from flexbe_core import initialize_flexbe_core +from flexbe_states.log_key_state import LogKeyState +from flexbe_states.operator_decision_state import OperatorDecisionState +from pyrobosim_flexbe_behaviors.traverse_sm import TraverseSM + +# Additional imports can be added inside the following tags +# [MANUAL_IMPORT] + + +# [/MANUAL_IMPORT] + + +class PatrolSM(Behavior): + """ + Define Patrol. + + Patrol rooms without concern for battery + """ + + def __init__(self, node): + super().__init__() + self.name = 'Patrol' + + # parameters of this behavior + + # Initialize ROS node information + initialize_flexbe_core(node) + + # references to used behaviors + self.add_behavior(TraverseSM, 'Closet', node) + self.add_behavior(TraverseSM, 'Dining1', node) + self.add_behavior(TraverseSM, 'Dining2', node) + self.add_behavior(TraverseSM, 'Kitchen', node) + self.add_behavior(TraverseSM, 'Office', node) + self.add_behavior(TraverseSM, 'Trash', node) + + # Additional initialization code can be added inside the following tags + # [MANUAL_INIT] + + + # [/MANUAL_INIT] + + # Behavior comments: + + def create(self): + """Create state machine.""" + # Root state machine + # x:339 y:165 + _state_machine = OperatableStateMachine(outcomes=['finished'], output_keys=['msg']) + _state_machine.userdata.msg = "unknown message" + + # Additional creation code can be added inside the following tags + # [MANUAL_CREATE] + + + # [/MANUAL_CREATE] + + with _state_machine: + # x:81 y:121 + OperatableStateMachine.add('Patrol', + OperatorDecisionState(outcomes=["patrol", "quit"], + hint="Continue patrol", + suggestion="patrol"), + transitions={'patrol': 'Office' # 293 90 -1 -1 -1 -1 + , 'quit': 'finished' # 275 196 181 174 -1 -1 + }, + autonomy={'patrol': Autonomy.Low, 'quit': Autonomy.Full}) + + # x:64 y:368 + OperatableStateMachine.add('Closet', + self.use_behavior(TraverseSM, 'Closet', + parameters={'target': "closet"}), + transitions={'finished': 'Patrol' # 104 274 131 367 136 174 + , 'failed': 'LogFailed' # 422 352 -1 -1 -1 -1 + }, + autonomy={'finished': Autonomy.Inherit, + 'failed': Autonomy.Inherit}, + remapping={'msg': 'msg'}) + + # x:503 y:556 + OperatableStateMachine.add('Dining1', + self.use_behavior(TraverseSM, 'Dining1', + parameters={'target': "dining"}), + transitions={'finished': 'Closet' # 313 488 -1 -1 -1 -1 + , 'failed': 'LogFailed' # 546 449 554 555 543 335 + }, + autonomy={'finished': Autonomy.Inherit, + 'failed': Autonomy.Inherit}, + remapping={'msg': 'msg'}) + + # x:676 y:92 + OperatableStateMachine.add('Dining2', + self.use_behavior(TraverseSM, 'Dining2', + parameters={'target': "dining"}), + transitions={'finished': 'Kitchen' # 973 151 -1 -1 1005 212 + , 'failed': 'LogFailed' # 607 213 -1 -1 -1 -1 + }, + autonomy={'finished': Autonomy.Inherit, + 'failed': Autonomy.Inherit}, + remapping={'msg': 'msg'}) + + # x:917 y:213 + OperatableStateMachine.add('Kitchen', + self.use_behavior(TraverseSM, 'Kitchen', + parameters={'target': "kitchen"}), + transitions={'finished': 'Trash' # 1051 360 -1 -1 983 452 + , 'failed': 'LogFailed' # 727 276 -1 -1 -1 -1 + }, + autonomy={'finished': Autonomy.Inherit, + 'failed': Autonomy.Inherit}, + remapping={'msg': 'msg'}) + + # x:503 y:282 + OperatableStateMachine.add('LogFailed', + LogKeyState(text="Patrol failure {}", + severity=2), + transitions={'done': 'Patrol' # 290 283 -1 -1 154 174 + }, + autonomy={'done': Autonomy.Off}, + remapping={'data': 'msg'}) + + # x:370 y:24 + OperatableStateMachine.add('Office', + self.use_behavior(TraverseSM, 'Office', + parameters={'target': "office"}), + transitions={'finished': 'Dining2' # 638 39 -1 -1 759 91 + , 'failed': 'LogFailed' # 445 188 -1 -1 -1 -1 + }, + autonomy={'finished': Autonomy.Inherit, + 'failed': Autonomy.Inherit}, + remapping={'msg': 'msg'}) + + # x:891 y:453 + OperatableStateMachine.add('Trash', + self.use_behavior(TraverseSM, 'Trash', + parameters={'target': "trash"}), + transitions={'finished': 'Dining1' # 817 568 -1 -1 681 585 + , 'failed': 'LogFailed' # 720 393 -1 -1 -1 -1 + }, + autonomy={'finished': Autonomy.Inherit, + 'failed': Autonomy.Inherit}, + remapping={'msg': 'msg'}) + + return _state_machine + + # Private functions can be added inside the following tags + # [MANUAL_FUNC] + + + # [/MANUAL_FUNC] diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/patrolcharge_sm.py b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/patrolcharge_sm.py new file mode 100644 index 0000000..f2efc1a --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/patrolcharge_sm.py @@ -0,0 +1,192 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +# Copyright 2024 Conner +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +########################################################### +# WARNING: Generated code! # +# ************************** # +# Manual changes may get lost if file is generated again. # +# Only code inside the [MANUAL] tags will be kept. # +########################################################### + +""" +Define PatrolCharge. + +Patrol and recharge as necessary + +Created on Mon Sep 23 2024 +@author: Conner +""" + + +from flexbe_core import Autonomy +from flexbe_core import Behavior +from flexbe_core import ConcurrencyContainer +from flexbe_core import Logger +from flexbe_core import OperatableStateMachine +from flexbe_core import PriorityContainer +from flexbe_core import initialize_flexbe_core +from flexbe_states.log_key_state import LogKeyState +from flexbe_states.log_state import LogState +from pyrobosim_flexbe_behaviors.patrol_sm import PatrolSM +from pyrobosim_flexbe_behaviors.traverse_sm import TraverseSM +from pyrobosim_flexbe_states.monitor_battery_state import MonitorBatteryState + +# Additional imports can be added inside the following tags +# [MANUAL_IMPORT] + + +# [/MANUAL_IMPORT] + + +class PatrolChargeSM(Behavior): + """ + Define PatrolCharge. + + Patrol and recharge as necessary + """ + + def __init__(self, node): + super().__init__() + self.name = 'PatrolCharge' + + # parameters of this behavior + + # Initialize ROS node information + initialize_flexbe_core(node) + + # references to used behaviors + self.add_behavior(TraverseSM, 'PatrolWithBattery/BatteryMaintain/Container/Traverse', node) + self.add_behavior(PatrolSM, 'PatrolWithBattery/Patrol', node) + + # Additional initialization code can be added inside the following tags + # [MANUAL_INIT] + + + # [/MANUAL_INIT] + + # Behavior comments: + + def create(self): + """Create state machine.""" + # Root state machine + # x:658 y:127, x:658 y:267 + _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) + + # Additional creation code can be added inside the following tags + # [MANUAL_CREATE] + + + # [/MANUAL_CREATE] + + # x:562 y:107, x:557 y:221 + _sm_container_0 = PriorityContainer(outcomes=['finished', 'failed'], + output_keys=['msg']) + + with _sm_container_0: + # x:87 y:45 + OperatableStateMachine.add('Recharge', + LogState(text="Begin recharge ...", + severity=Logger.REPORT_INFO), + transitions={'done': 'Traverse' # 241 71 -1 -1 250 120 + }, + autonomy={'done': Autonomy.Off}) + + # x:156 y:121 + OperatableStateMachine.add('Traverse', + self.use_behavior(TraverseSM, 'PatrolWithBattery/BatteryMaintain/Container/Traverse', + parameters={'target': "charger_dock"}), + transitions={'finished': 'finished' # 451 132 -1 -1 -1 -1 + , 'failed': 'failed' # 448 189 334 161 -1 -1 + }, + autonomy={'finished': Autonomy.Low, 'failed': Autonomy.Low}, + remapping={'msg': 'msg'}) + + # x:333 y:290 + _sm_batterymaintain_1 = OperatableStateMachine(outcomes=['failed']) + + with _sm_batterymaintain_1: + # x:126 y:87 + OperatableStateMachine.add('CheckBattery', + MonitorBatteryState(low_battery_level=30.0, + high_battery_level=105.0, + state_topic='robot/robot_state', + timeout=2.0), + transitions={'battery_level': 'Container' # 326 107 -1 -1 -1 -1 + , 'failed': 'failed' # 207 240 171 140 -1 -1 + }, + autonomy={'battery_level': Autonomy.Off, + 'failed': Autonomy.Off}, + remapping={'battery_level': 'battery_level'}) + + # x:398 y:90 + OperatableStateMachine.add('Container', + _sm_container_0, + transitions={'finished': 'CheckBattery' # 328 222 423 149 228 140 + , 'failed': 'failed' # 442 232 457 149 -1 -1 + }, + autonomy={'finished': Autonomy.Inherit, + 'failed': Autonomy.Inherit}, + remapping={'msg': 'msg'}) + + # x:621 y:77, x:631 y:283, x:603 y:384, x:616 y:482 + _sm_patrolwithbattery_2 = ConcurrencyContainer(outcomes=['finished', 'failed'], + output_keys=['msg'], + conditions=[('failed', [('BatteryMaintain', 'failed')]), + ('finished', [('Patrol', 'finished')]) + ]) + + with _sm_patrolwithbattery_2: + # x:132 y:62 + OperatableStateMachine.add('Patrol', + self.use_behavior(PatrolSM, 'PatrolWithBattery/Patrol'), + transitions={'finished': 'finished'}, + autonomy={'finished': Autonomy.Inherit}, + remapping={'msg': 'msg'}) + + # x:163 y:269 + OperatableStateMachine.add('BatteryMaintain', + _sm_batterymaintain_1, + transitions={'failed': 'failed'}, + autonomy={'failed': Autonomy.Inherit}) + + with _state_machine: + # x:290 y:93 + OperatableStateMachine.add('PatrolWithBattery', + _sm_patrolwithbattery_2, + transitions={'finished': 'finished' # 547 125 -1 -1 -1 -1 + , 'failed': 'LogFailed' # 441 172 -1 -1 -1 -1 + }, + autonomy={'finished': Autonomy.High, + 'failed': Autonomy.Inherit}, + remapping={'msg': 'msg'}) + + # x:477 y:188 + OperatableStateMachine.add('LogFailed', + LogKeyState(text="Failed {}", + severity=Logger.REPORT_ERROR), + transitions={'done': 'failed' # 610 246 -1 -1 -1 -1 + }, + autonomy={'done': Autonomy.Off}, + remapping={'data': 'msg'}) + + return _state_machine + + # Private functions can be added inside the following tags + # [MANUAL_FUNC] + + + # [/MANUAL_FUNC] diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/test_navigate_sm.py b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/test_navigate_sm.py index ca8d460..12222e7 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/test_navigate_sm.py +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/test_navigate_sm.py @@ -108,19 +108,11 @@ def create(self): }, autonomy={'done': Autonomy.Full}) - # x:1086 y:90 - OperatableStateMachine.add('LogCounter', - LogState(text="Arrived at counter", - severity=2), - transitions={'done': 'finished' # 1221 89 -1 -1 -1 -1 - }, - autonomy={'done': Autonomy.High}) - # x:629 y:102 OperatableStateMachine.add('LogDesk', LogState(text="Arrived at desk", severity=2), - transitions={'done': 'NavigateCounter' # 753 104 -1 -1 -1 -1 + transitions={'done': 'NavigatePantry' # 753 104 -1 -1 -1 -1 }, autonomy={'done': Autonomy.High}) @@ -132,6 +124,14 @@ def create(self): }, autonomy={'done': Autonomy.Full}) + # x:1086 y:90 + OperatableStateMachine.add('LogPantry', + LogState(text="Arrived at pantry", + severity=2), + transitions={'done': 'finished' # 1221 89 -1 -1 -1 -1 + }, + autonomy={'done': Autonomy.High}) + # x:908 y:498 OperatableStateMachine.add('LogTimeout', LogState(text="time out error", @@ -140,43 +140,45 @@ def create(self): }, autonomy={'done': Autonomy.Full}) - # x:806 y:103 - OperatableStateMachine.add('NavigateCounter', - NavigateActionState(target_location='counter', + # x:345 y:144 + OperatableStateMachine.add('NavigateDesk', + NavigateActionState(target_location='desk', robot_name=robot_name, action_topic=action_topic, server_timeout=5.0, navigate_timeout=None), - transitions={'done': 'LogCounter' # 1053 96 -1 -1 -1 -1 - , 'planning_failed': 'LogFailed' # 1055 198 -1 -1 -1 -1 - , 'motion_failed': 'LogFailed' # 1055 198 -1 -1 -1 -1 - , 'canceled': 'LogCanceled' # 929 275 -1 -1 -1 -1 - , 'timeout': 'LogTimeout' # 850 448 853 156 -1 -1 + transitions={'done': 'LogDesk' # 575 134 -1 -1 -1 -1 + , 'planning_failed': 'LogFailed' # 814 255 -1 -1 -1 -1 + , 'motion_failed': 'LogFailed' # 814 255 -1 -1 -1 -1 + , 'canceled': 'LogCanceled' # 761 283 -1 -1 -1 -1 + , 'timeout': 'LogTimeout' # 718 478 -1 -1 907 540 }, autonomy={'done': Autonomy.Off, 'planning_failed': Autonomy.Off, 'motion_failed': Autonomy.Off, 'canceled': Autonomy.Off, - 'timeout': Autonomy.Off}) + 'timeout': Autonomy.Off}, + remapping={'msg': 'msg'}) - # x:345 y:144 - OperatableStateMachine.add('NavigateDesk', - NavigateActionState(target_location='desk', + # x:806 y:103 + OperatableStateMachine.add('NavigatePantry', + NavigateActionState(target_location='pantry', robot_name=robot_name, action_topic=action_topic, server_timeout=5.0, navigate_timeout=None), - transitions={'done': 'LogDesk' # 575 134 -1 -1 -1 -1 - , 'planning_failed': 'LogFailed' # 814 255 -1 -1 -1 -1 - , 'motion_failed': 'LogFailed' # 814 255 -1 -1 -1 -1 - , 'canceled': 'LogCanceled' # 761 283 -1 -1 -1 -1 - , 'timeout': 'LogTimeout' # 718 478 -1 -1 907 540 + transitions={'done': 'LogPantry' # 1053 96 -1 -1 -1 -1 + , 'planning_failed': 'LogFailed' # 1055 198 -1 -1 -1 -1 + , 'motion_failed': 'LogFailed' # 1055 198 -1 -1 -1 -1 + , 'canceled': 'LogCanceled' # 929 275 -1 -1 -1 -1 + , 'timeout': 'LogTimeout' # 850 448 853 156 -1 -1 }, autonomy={'done': Autonomy.Off, 'planning_failed': Autonomy.Off, 'motion_failed': Autonomy.Off, 'canceled': Autonomy.Off, - 'timeout': Autonomy.Off}) + 'timeout': Autonomy.Off}, + remapping={'msg': 'msg'}) return _state_machine diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/test_pick_place_sm.py b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/test_pick_place_sm.py index a05a258..c404561 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/test_pick_place_sm.py +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/test_pick_place_sm.py @@ -63,9 +63,9 @@ def __init__(self, node): self.name = 'test pick place' # parameters of this behavior - self.add_parameter('location', 'counter0_left') - self.add_parameter('object', 'banana') - self.add_parameter('return_location', 'desk0') + self.add_parameter('location', 'pantry') + self.add_parameter('object', 'bread') + self.add_parameter('return_location', 'desk') # Initialize ROS node information initialize_flexbe_core(node) @@ -76,12 +76,6 @@ def __init__(self, node): # [MANUAL_INIT] - - - - - - # [/MANUAL_INIT] # Behavior comments: @@ -101,12 +95,6 @@ def create(self): # [MANUAL_CREATE] - - - - - - # [/MANUAL_CREATE] with _state_machine: @@ -183,7 +171,8 @@ def create(self): 'planning_failed': Autonomy.Off, 'motion_failed': Autonomy.Off, 'canceled': Autonomy.Off, - 'timeout': Autonomy.Off}) + 'timeout': Autonomy.Off}, + remapping={'msg': 'msg'}) # x:827 y:52 OperatableStateMachine.add('NavigateReturn', @@ -202,7 +191,8 @@ def create(self): 'planning_failed': Autonomy.Off, 'motion_failed': Autonomy.Off, 'canceled': Autonomy.Off, - 'timeout': Autonomy.Off}) + 'timeout': Autonomy.Off}, + remapping={'msg': 'msg'}) # x:536 y:47 OperatableStateMachine.add('PickObject', @@ -232,10 +222,4 @@ def create(self): # [MANUAL_FUNC] - - - - - - # [/MANUAL_FUNC] diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/test_plan_path_sm.py b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/test_plan_path_sm.py index efaa32e..46eda68 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/test_plan_path_sm.py +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/test_plan_path_sm.py @@ -72,10 +72,6 @@ def __init__(self, node): # [MANUAL_INIT] - - - - # [/MANUAL_INIT] # Behavior comments: @@ -85,23 +81,17 @@ def create(self): # Root state machine # x:1232 y:54, x:1214 y:117 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) - _state_machine.userdata.destination = 'table_source' - _state_machine.userdata.table = 'table_sink' - _state_machine.userdata.desk = 'desk0' + _state_machine.userdata.destination = 'dumpster' # Additional creation code can be added inside the following tags # [MANUAL_CREATE] - - - - # [/MANUAL_CREATE] with _state_machine: # x:128 y:59 - OperatableStateMachine.add('PlanCounter', + OperatableStateMachine.add('PlanDestination', PlanPathState(action_topic='robot/plan_path', timeout=2.0), transitions={'done': 'FollowPath' # 428 72 -1 -1 -1 -1 @@ -137,8 +127,4 @@ def create(self): # [MANUAL_FUNC] - - - - # [/MANUAL_FUNC] diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/through_door_sm.py b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/through_door_sm.py new file mode 100644 index 0000000..62f0ad1 --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/through_door_sm.py @@ -0,0 +1,224 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +# Copyright 2024 conner +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +########################################################### +# WARNING: Generated code! # +# ************************** # +# Manual changes may get lost if file is generated again. # +# Only code inside the [MANUAL] tags will be kept. # +########################################################### + +""" +Define Through Door. + +Go through a door, opening if required + +Created on Sun Sep 08 2024 +@author: conner +""" + + +from flexbe_core import Autonomy +from flexbe_core import Behavior +from flexbe_core import ConcurrencyContainer +from flexbe_core import Logger +from flexbe_core import OperatableStateMachine +from flexbe_core import PriorityContainer +from flexbe_core import initialize_flexbe_core +from flexbe_states.operator_decision_state import OperatorDecisionState +from pyrobosim_flexbe_states.check_door_state import CheckDoorState +from pyrobosim_flexbe_states.door_action_state import DoorActionState +from pyrobosim_flexbe_states.follow_path_state import FollowPathState +from pyrobosim_flexbe_states.plan_path_state import PlanPathState + +# Additional imports can be added inside the following tags +# [MANUAL_IMPORT] + + +# [/MANUAL_IMPORT] + + +class ThroughDoorSM(Behavior): + """ + Define Through Door. + + Go through a door, opening if required + """ + + def __init__(self, node): + super().__init__() + self.name = 'Through Door' + + # parameters of this behavior + + # Initialize ROS node information + initialize_flexbe_core(node) + + # references to used behaviors + + # Additional initialization code can be added inside the following tags + # [MANUAL_INIT] + + + # [/MANUAL_INIT] + + # Behavior comments: + + # 0 171 407 + # If not, plan path to door and open + + # 0 419 20 + # If we can plan path, then go through the door. + + # 0 202 269 + # If we don't like round about path, give option to go to relevant door. + + def create(self): + """Create state machine.""" + # Root state machine + # x:957 y:80, x:1009 y:269 + _state_machine = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['room', 'hallway'], output_keys=['msg']) + _state_machine.userdata.room = 'closet' + _state_machine.userdata.hallway = 'hall_dining_closet' + _state_machine.userdata.msg = '' + + # Additional creation code can be added inside the following tags + # [MANUAL_CREATE] + + + # [/MANUAL_CREATE] + + with _state_machine: + # x:127 y:57 + OperatableStateMachine.add('PlanPath', + PlanPathState(action_topic='robot/plan_path', + timeout=10.0), + transitions={'done': 'ConfirmGo' # 306 86 -1 -1 -1 -1 + , 'failed': 'PlanDoor' # 190 328 -1 -1 -1 -1 + }, + autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off}, + remapping={'goal': 'room', + 'msg': 'msg', + 'path': 'path_to_next'}) + + # x:712 y:261 + OperatableStateMachine.add('CheckOpen', + CheckDoorState(call_timeout=3.0, + wait_timeout=3.0, + service_name='request_world_state', + robot=''), + transitions={'open': 'PlanPath' # 399 219 -1 -1 219 110 + , 'closed': 'OpenDoor' # 917 311 -1 -1 -1 -1 + , 'failed': 'failed' # 932 282 -1 -1 -1 -1 + }, + autonomy={'open': Autonomy.Low, + 'closed': Autonomy.Low, + 'failed': Autonomy.High}, + remapping={'name': 'hallway', 'msg': 'msg'}) + + # x:346 y:66 + OperatableStateMachine.add('ConfirmGo', + OperatorDecisionState(outcomes=['go', 'door', 'failed'], + hint='go if path is desired', + suggestion='go'), + transitions={'go': 'GoRoom' # 512 93 -1 -1 -1 -1 + , 'door': 'PlanDoor' # 308 233 -1 -1 -1 -1 + , 'failed': 'failed' # 733 225 -1 -1 -1 -1 + }, + autonomy={'go': Autonomy.Low, + 'door': Autonomy.Full, + 'failed': Autonomy.Full}) + + # x:506 y:316 + OperatableStateMachine.add('GoDoor', + FollowPathState(action_topic='robot/follow_path', + server_timeout=2.0), + transitions={'done': 'CheckOpen' # 673 312 -1 -1 -1 -1 + , 'failed': 'RetryDoor' # 594 399 -1 -1 610 449 + }, + autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off}, + remapping={'path': 'path_to_door', 'msg': 'msg'}) + + # x:562 y:69 + OperatableStateMachine.add('GoRoom', + FollowPathState(action_topic='robot/follow_path', + server_timeout=2.0), + transitions={'done': 'finished' # 831 91 -1 -1 -1 -1 + , 'failed': 'RetryNav' # 764 151 -1 -1 -1 -1 + }, + autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off}, + remapping={'path': 'path_to_next', 'msg': 'msg'}) + + # x:852 y:357 + OperatableStateMachine.add('OpenDoor', + DoorActionState(action_type='open', + robot_name='robot', + action_topic='/execute_action', + timeout=2.0), + transitions={'done': 'CheckOpen' # 794 364 -1 -1 -1 -1 + , 'failed': 'RetryDoor' # 784 424 -1 -1 706 463 + }, + autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off}, + remapping={'msg': 'msg'}) + + # x:252 y:346 + OperatableStateMachine.add('PlanDoor', + PlanPathState(action_topic='robot/plan_path', + timeout=10.0), + transitions={'done': 'GoDoor' # 457 346 -1 -1 -1 -1 + , 'failed': 'RetryDoor' # 442 428 -1 -1 578 463 + }, + autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off}, + remapping={'goal': 'hallway', + 'msg': 'msg', + 'path': 'path_to_door'}) + + # x:579 y:450 + OperatableStateMachine.add('RetryDoor', + OperatorDecisionState(outcomes=['door', + 'room', + 'open', + 'failed'], + hint="Retry the door", + suggestion='door'), + transitions={'door': 'PlanDoor' # 333 459 578 476 328 399 + , 'room': 'PlanPath' # 187 505 578 487 152 110 + , 'open': 'OpenDoor' # 838 460 -1 -1 895 410 + , 'failed': 'failed' # 979 475 706 487 1021 300 + }, + autonomy={'door': Autonomy.Low, + 'room': Autonomy.Full, + 'open': Autonomy.Full, + 'failed': Autonomy.Full}) + + # x:790 y:151 + OperatableStateMachine.add('RetryNav', + OperatorDecisionState(outcomes=['retry', 'failed'], + hint="Retry", + suggestion='retry'), + transitions={'retry': 'PlanPath' # 505 200 789 186 245 110 + , 'failed': 'failed' # 888 226 -1 -1 1005 268 + }, + autonomy={'retry': Autonomy.Low, 'failed': Autonomy.Full}) + + return _state_machine + + # Private functions can be added inside the following tags + # [MANUAL_FUNC] + + + # [/MANUAL_FUNC] diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/traverse_sm.py b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/traverse_sm.py new file mode 100644 index 0000000..7b9f50f --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/traverse_sm.py @@ -0,0 +1,150 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +# Copyright 2024 conner +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +########################################################### +# WARNING: Generated code! # +# ************************** # +# Manual changes may get lost if file is generated again. # +# Only code inside the [MANUAL] tags will be kept. # +########################################################### + +""" +Define Traverse. + +Traverse the world opening doors as required + +Created on Sun Sep 08 2024 +@author: conner +""" + + +from flexbe_core import Autonomy +from flexbe_core import Behavior +from flexbe_core import ConcurrencyContainer +from flexbe_core import Logger +from flexbe_core import OperatableStateMachine +from flexbe_core import PriorityContainer +from flexbe_core import initialize_flexbe_core +from flexbe_states.log_key_state import LogKeyState +from pyrobosim_flexbe_behaviors.go_beh_sm import GoBehSM +from pyrobosim_flexbe_behaviors.through_door_sm import ThroughDoorSM +from pyrobosim_flexbe_states.next_room_state import NextRoomState + +# Additional imports can be added inside the following tags +# [MANUAL_IMPORT] + + +# [/MANUAL_IMPORT] + + +class TraverseSM(Behavior): + """ + Define Traverse. + + Traverse the world opening doors as required + """ + + def __init__(self, node): + super().__init__() + self.name = 'Traverse' + + # parameters of this behavior + self.add_parameter('target', 'charger') + + # Initialize ROS node information + initialize_flexbe_core(node) + + # references to used behaviors + self.add_behavior(GoBehSM, 'GoLocation', node) + self.add_behavior(ThroughDoorSM, 'ThruDoor', node) + + # Additional initialization code can be added inside the following tags + # [MANUAL_INIT] + + + # [/MANUAL_INIT] + + # Behavior comments: + + def create(self): + """Create state machine.""" + # Root state machine + # x:801 y:105, x:838 y:264 + _state_machine = OperatableStateMachine(outcomes=['finished', 'failed'], output_keys=['msg']) + _state_machine.userdata.goal = self.target + _state_machine.userdata.msg = "Unknown message" + + # Additional creation code can be added inside the following tags + # [MANUAL_CREATE] + + + # [/MANUAL_CREATE] + + with _state_machine: + # x:227 y:85 + OperatableStateMachine.add('GoLocation', + self.use_behavior(GoBehSM, 'GoLocation', + parameters={'target': "desk"}), + transitions={'finished': 'finished' # 607 109 -1 -1 -1 -1 + , 'failed': 'NextRoom' # 307 234 -1 -1 -1 -1 + }, + autonomy={'finished': Autonomy.High, + 'failed': Autonomy.Inherit}, + remapping={'goal': 'goal', 'msg': 'msg'}) + + # x:467 y:295 + OperatableStateMachine.add('LogNext', + LogKeyState(text="Next room '{}'", + severity=Logger.REPORT_INFO), + transitions={'done': 'ThruDoor' # 620 322 -1 -1 658 375 + }, + autonomy={'done': Autonomy.Off}, + remapping={'data': 'room'}) + + # x:363 y:218 + OperatableStateMachine.add('NextRoom', + NextRoomState(world='world4', + package_name='delib_ws_worlds', + sub_folder='worlds', + state_topic='robot/robot_state', + timeout=2.0), + transitions={'done': 'LogNext' # 412 304 401 271 -1 -1 + , 'failed': 'failed' # 680 263 499 268 -1 -1 + }, + autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off}, + remapping={'goal': 'goal', + 'room': 'room', + 'hallway': 'hallway'}) + + # x:586 y:376 + OperatableStateMachine.add('ThruDoor', + self.use_behavior(ThroughDoorSM, 'ThruDoor'), + transitions={'finished': 'GoLocation' # 112 348 -1 -1 -1 -1 + , 'failed': 'failed' # 823 392 -1 -1 846 295 + }, + autonomy={'finished': Autonomy.High, 'failed': Autonomy.Off}, + remapping={'room': 'room', + 'hallway': 'hallway', + 'msg': 'msg'}) + + return _state_machine + + # Private functions can be added inside the following tags + # [MANUAL_FUNC] + + + # [/MANUAL_FUNC] diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/setup.py b/technologies/FlexBE/pyrobosim_flexbe_behaviors/setup.py index db0ead5..b12cb51 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_behaviors/setup.py +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/setup.py @@ -14,14 +14,13 @@ ], install_requires=['setuptools'], zip_safe=True, - maintainer='phil', - maintainer_email='philsplus@gmail.com', - description='TODO: Package description', - license='TODO: License declaration', + maintainer='David Conner', + maintainer_email='robotics@cnu.edu', + description='Demonstration behaviors for FlexBE using Pyrobosim', + license='Apache 2.0', tests_require=['pytest'], entry_points={ 'console_scripts': [ - 'example_behavior_sm = pyrobosim_flexbe_behaviors.example_behavior_sm', ], }, ) diff --git a/technologies/FlexBE/pyrobosim_flexbe_states/package.xml b/technologies/FlexBE/pyrobosim_flexbe_states/package.xml index d54231c..63f698f 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_states/package.xml +++ b/technologies/FlexBE/pyrobosim_flexbe_states/package.xml @@ -18,6 +18,7 @@ rclpy flexbe_core + pyrobosim_flexbe_utilities ament_copyright ament_flake8 diff --git a/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/__init__.py b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/__init__.py index e69de29..56709c5 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/__init__.py +++ b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/__init__.py @@ -0,0 +1 @@ +"""pyrobosim_flexbe_states module.""" diff --git a/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/check_door_state.py b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/check_door_state.py new file mode 100644 index 0000000..a2712cd --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/check_door_state.py @@ -0,0 +1,206 @@ +#!/usr/bin/env python + +# Copyright 2024 Christopher Newport University +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Check pyrobosim door FlexBE state.""" + +from flexbe_core import EventState, Logger +from flexbe_core.proxy import ProxyServiceCaller + +from pyrobosim_msgs.srv import RequestWorldState + +from rclpy.duration import Duration + + +class CheckDoorState(EventState): + """ + This state checks the status of given "openable" in pyrobosim. + + Elements defined here for UI + Parameters + -- call_timeout float Timeout for completion (default: 3.0 seconds) + -- wait_timeout float Duration to wait for service to become available (default: 3.0 seconds) + -- service_name string Service name (default: `request_world_state`) + -- robot string Robot name (default: '' request world state) + + Outputs + <= open Service call returned result as expected + <= closed Failed to make service call successfully + <= failed Service call did not return timely result + + User data + ># name string Name of openable (e.g. 'hall_room1_room2', 'fridge_storage') + #> msg string Output message + """ + + def __init__(self, call_timeout=3.0, wait_timeout=3.0, service_name='request_world_state', robot=''): + """Declare outcomes, input_keys, and output_keys by calling the EventState super constructor.""" + super().__init__(outcomes=['open', 'closed', 'failed'], + input_keys=['name'], + output_keys=['msg']) + + # No longer needed in 4.0+ ProxyServiceCaller.initialize(CheckDoorState._node) + + # Store state parameters for later use. + self._call_timeout = Duration(seconds=call_timeout) + self._wait_timeout = Duration(seconds=wait_timeout) + + # The constructor is called when building the state machine, not when actually starting the behavior. + # Thus, we cannot save the starting time now and will do so later. + self._start_time = None + self._return = None # Track the outcome so we can detect if transition is blocked + self._service_called = False + + self._srv_topic = service_name + self._srv_result = None + + self._srv_request = RequestWorldState.Request() + self._srv_request.robot = robot + + self._error = None + self._srv = None + + def on_start(self): + """Execute when behavior starts.""" + # Set up the proxy now, but do not wait on the service just yet + Logger.localinfo(f" on_start '{self}' - '{self.path}' ...") + self._srv = ProxyServiceCaller({self._srv_topic: RequestWorldState}, wait_duration=0.0) + + def on_stop(self): + """Execute when behavior stops.""" + # Remove the proxy client if no longer in use + ProxyServiceCaller.remove_client(self._srv_topic) + self._srv = None + Logger.localinfo(f" on_stop '{self}' - '{self.path}' ...") + + def on_pause(self): + """Execute each time this state is paused.""" + Logger.localinfo(f"on_pause '{self}' - '{self.path}' ...") + + def on_resume(self, userdata): + """Execute each time this state is resumed.""" + Logger.localinfo(f"on_resume '{self}' - '{self.path}' ...") + + def execute(self, userdata): + """ + Execute this method periodically while the state is active. + + If no outcome is returned, the state will stay active. + """ + if self._return: + # We have completed the state, and therefore must be blocked by autonomy level + return self._return + + if self._service_called: + # Waiting for result. + # We will do this in a non-blocking way + if self._srv.done(self._srv_topic): + result = self._srv.result(self._srv_topic) # grab result + self._process_result(result, userdata.name) + if self._return == 'failed': + userdata.msg = f"{self._name}: world state did not find '{userdata.name}'!" + return self._return + else: + elapsed = self._node.get_clock().now() - self._start_time + if elapsed > self._call_timeout: + # Failed to return call in timely manner + self._return = 'failed' + userdata.msg = f"'{self._name}': Service '{self._srv_topic}' call timed out!" + else: + # Waiting for service to become available in non-blocking manner + if self._srv.is_available(self._srv_topic, wait_duration=0.0): + Logger.localinfo(f"'{self._name}': Service '{self._srv_topic}' is now available - making service call!") + self._do_service_call() + # Process the result on next execute call (so some delay) + else: + elapsed = self._node.get_clock().now() - self._start_time + if elapsed > self._wait_timeout: + # Failed to return call in timely manner + self._return = 'failed' + userdata.msg = f"'{self._name}': Service '{self._srv_topic}' is unavailable!" + + return self._return + + def on_enter(self, userdata): + """ + Call this method when the state becomes active. + + i.e. a transition from another state to this one is taken. + """ + Logger.localinfo(f" on_enter '{self}' - '{self.path}' ...") + if 'name' in userdata and isinstance(userdata.name, str): + userdata.msg = '' + self._start_time = self._node.get_clock().now() + self._return = None # reset the completion flag + self._service_called = False + try: + # Don't block just yet + if self._srv.is_available(self._srv_topic, wait_duration=0.0): + self._do_service_call() + else: + Logger.logwarn(f"'{self._name}': Service '{self._srv_topic}' is not yet available ...") + except Exception as exc: # pylint: disable=W0703 + Logger.logerr(f"'{self._name}': Service '{self._srv_topic}' exception {type(exc)} - {str(exc)}") + self._return = 'failed' + else: + userdata.msg = f"'{self}' - userdata must contain 'name' field!" + self._return = 'failed' + return + + def on_exit(self, userdata): + """Call when state deactivates.""" + # make sure to reset the data from prior executions + Logger.localinfo(f" on_exit '{self}' - '{self.path}' ...") + + def _do_service_call(self): + """Make the service call using async non-blocking.""" + try: + Logger.localinfo(f"'{self._name}': Calling service '{self._srv_topic}' ...") + self._srv_result = self._srv.call_async(self._srv_topic, self._srv_request, wait_duration=0.0) + self._start_time = self._node.get_clock().now() # Reset timer for call timeout + self._service_called = True + except Exception as exc: + Logger.logerr(f"'{self._name}': Service '{self._srv_topic}' exception {type(exc)} - {str(exc)}") + raise exc + + def _process_result(self, result, name): + """Look for name in result.""" + world_state = result.state + if name.startswith('hall_'): + # This is a door between two rooms + possible_rooms = tuple(name[len('hall_'):].split('_')) + alternate_name = f'hall_{possible_rooms[1]}_{possible_rooms[0]}' + # Check if hallway door + for loc in world_state.hallways: + if loc.name == name or loc.name == alternate_name: + if loc.is_open: + self._return = 'open' + else: + self._return = 'closed' + return + + endings = ['_dock', '_tabletop', '_disposal', '_storage'] + location = next((name.replace(ending, '') for ending in endings if name.endswith(ending)), name) + + for loc in world_state.locations: + if loc.name == location: + if loc.is_open: + self._return = 'open' + else: + self._return = 'closed' + return + + Logger.localinfo(f"'{name}' not found at location '{location}' or hallway!") + self._return = 'failed' diff --git a/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/detect_local_objects_state.py b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/detect_local_objects_state.py index bb033d9..3a69644 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/detect_local_objects_state.py +++ b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/detect_local_objects_state.py @@ -14,7 +14,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""FlexBE State to command PyRoboSim robot to plan a path to target location.""" +"""FlexBE State to request local objects for PyRoboSim robot.""" from action_msgs.msg import GoalStatus @@ -22,15 +22,15 @@ from flexbe_core.proxy import ProxyActionClient, ProxyServiceCaller from pyrobosim_msgs.action import ExecuteTaskAction -from pyrobosim_msgs.srv import RequestWorldState from pyrobosim_msgs.msg import ExecutionResult, TaskAction +from pyrobosim_msgs.srv import RequestWorldState from rclpy.duration import Duration class DetectLocalObjectsState(EventState): """ - FlexBE state to request local objects for PyRoboSim robot + FlexBE state to request local objects for PyRoboSim robot. Elements defined here for UI Parameters @@ -78,17 +78,29 @@ def __init__(self, filter=None, self._service_called = False def on_start(self): + """Execute when behavior starts.""" # Set up the proxy now, but do not wait on the service just yet + Logger.localinfo(f" on_start '{self}' - '{self.path}' ...") self._srv_client = ProxyServiceCaller({self._srv_topic: RequestWorldState}, wait_duration=0.0) self._act_client = ProxyActionClient({self._action_topic: ExecuteTaskAction}, wait_duration=0.0) # no need to wait here, we'll check on_enter def on_stop(self): + """Execute when behavior stops.""" # Remove the proxy client if no longer in use ProxyServiceCaller.remove_client(self._srv_topic) ProxyActionClient.remove_client(self._action_topic) self._srv_client = None self._act_client = None + Logger.localinfo(f" on_stop '{self}' - '{self.path}' ...") + + def on_pause(self): + """Execute each time this state is paused.""" + Logger.localinfo(f"on_pause '{self}' - '{self.path}' ...") + + def on_resume(self, userdata): + """Execute each time this state is resumed.""" + Logger.localinfo(f"on_resume '{self}' - '{self.path}' ...") def execute(self, userdata): """ @@ -109,9 +121,9 @@ def execute(self, userdata): userdata.items = self._process_srv_response(result) Logger.localinfo(f" returned '{self._return}' with {len(userdata.items) if userdata.items else 0}") elif self._node.get_clock().now().nanoseconds - self._srv_start_time.nanoseconds > self._call_timeout.nanoseconds: - # Failed to return call in timely manner - self._return = 'failed' - Logger.logerr(f"{self._name}: Service {self._srv_topic} call timed out!") + # Failed to return call in timely manner + self._return = 'failed' + Logger.logerr(f"'{self._name}': Service '{self._srv_topic}' call timed out!") return self._return # We have not finished detection yet @@ -127,14 +139,14 @@ def execute(self, userdata): if result_status == ExecutionResult.SUCCESS: if self._srv_client.is_available(self._srv_topic, wait_duration=0.0): # Non-blocking check for availability - Logger.localinfo(f"{self} - detection completed - now request items from service result") + Logger.localinfo(f"'{self}' - detection completed - now request items from service result") self._do_service_call() elif self._srv_start_time is None: - Logger.localinfo(f"{self} - detection completed - wait for world state server to become available") + Logger.localinfo(f"'{self}' - detection completed - wait for world state server to become available") self._srv_start_time = self._node.get_clock().now() # Reset timer for call timeout elif self._node.get_clock().now().nanoseconds - self._srv_start_time.nanoseconds > self._call_timeout.nanoseconds: # Failed to return call in timely manner - Logger.logerr(f"{self._name}: Service {self._srv_topic} is not available!") + Logger.logerr(f"'{self._name}': Service '{self._srv_topic}' is not available!") self._return = 'failed' # Otherwise waiting for service to become available @@ -146,7 +158,7 @@ def execute(self, userdata): Logger.loginfo(f" '{self}' : '{self._action_topic}' - detection goal was aborted! ") self._return = 'failed' except Exception as exc: # pylint: disable=W0703 - Logger.logerr(f"{self._name}: {self._action_topic} exception {type(exc)} - {str(exc)}") + Logger.logerr(f"'{self._name}': '{self._action_topic}' exception {type(exc)} - {str(exc)}") self._return = 'failed' # If the action has not yet finished, None outcome will be returned and the state stays active. @@ -155,12 +167,12 @@ def execute(self, userdata): def _do_service_call(self): """Make the service call using async non-blocking.""" try: - Logger.localinfo(f"{self._name}: Calling service {self._srv_topic} ...") + Logger.localinfo(f"'{self._name}': Calling service '{self._srv_topic}' ...") srv_request = RequestWorldState.Request(robot=self._robot_name) - self._srv_client.call_async(self._srv_topic, srv_request, wait_duration=self._call_timeout.nanoseconds*1e-9) + self._srv_client.call_async(self._srv_topic, srv_request, wait_duration=self._call_timeout.nanoseconds * 1e-9) self._service_called = True except Exception as exc: - Logger.logerr(f"{self._name}: Service {self._srv_topic} exception {type(exc)} - {str(exc)}") + Logger.logerr(f"'{self._name}': Service '{self._srv_topic}' exception {type(exc)} - {str(exc)}") raise exc def _process_srv_response(self, response): @@ -178,7 +190,7 @@ def _process_srv_response(self, response): Logger.localinfo(f"Robot '{self._robot_name}' at location '{robot.last_visited_location}'") - local_objects = [] + local_objects = [] for obj in response.state.objects: if obj.parent == robot.last_visited_location: if self._filter is None: @@ -191,6 +203,7 @@ def _process_srv_response(self, response): def on_enter(self, userdata): """Call when state becomes active.""" # make sure to reset the data from prior executions + Logger.localinfo(f" on_enter '{self}' - '{self.path}' ...") self._srv_start_time = None # Reset timer for call timeout self._return = None self._service_called = False @@ -203,11 +216,11 @@ def on_enter(self, userdata): goal = ExecuteTaskAction.Goal() if self._filter is None: goal.action = TaskAction(robot=self._robot_name, - type="detect", + type='detect', ) else: goal.action = TaskAction(robot=self._robot_name, - type="detect", + type='detect', object=self._filter ) self._act_client.send_goal(self._action_topic, goal, wait_duration=self._server_timeout_sec) @@ -232,10 +245,12 @@ def on_exit(self, userdata): self._return = 'failed' else: status_string = self._act_client.get_status_string(self._action_topic) - Logger.loginfo(f" '{self}' : '{self._action_topic}' - Requested to cancel an active plan request ({status_string}).") + Logger.loginfo(f" '{self}' : '{self._action_topic}' - Requested to cancel " + f"an active plan request ('{status_string}').") # Local message are shown in terminal but not the UI if self._return == 'done': - Logger.localinfo(f'Successfully retrieved detected items.') + Logger.localinfo('Successfully retrieved detected items.') else: Logger.localwarn('Failed to detect items.') + Logger.localinfo(f" on_exit '{self}' - '{self.path}' ...") diff --git a/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/detect_objects_state.py b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/detect_objects_state.py index b63d69c..babec03 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/detect_objects_state.py +++ b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/detect_objects_state.py @@ -29,7 +29,7 @@ class DetectObjectsState(EventState): """ - FlexBE state to request local objects for PyRoboSim robot + FlexBE state to request local objects for PyRoboSim robot. Elements defined here for UI Parameters @@ -68,14 +68,26 @@ def __init__(self, filter=None, self._service_called = False def on_start(self): + """Call when behavior starts.""" # Set up the proxy now, but do not wait on the service just yet + Logger.localinfo(f" on_start '{self}' - '{self.path}' ...") self._act_client = ProxyActionClient({self._action_topic: DetectObjects}, wait_duration=0.0) # no need to wait here, we'll check on_enter def on_stop(self): + """Call when behavior stops.""" # Remove the proxy client if no longer in use ProxyActionClient.remove_client(self._action_topic) self._act_client = None + Logger.localinfo(f" on_stop '{self}' - '{self.path}' ...") + + def on_pause(self): + """Execute each time this state is paused.""" + Logger.localinfo(f"on_pause '{self}' - '{self.path}' ...") + + def on_resume(self, userdata): + """Execute each time this state is resumed.""" + Logger.localinfo(f"on_resume '{self}' - '{self.path}' ...") def execute(self, userdata): """ @@ -98,10 +110,10 @@ def execute(self, userdata): userdata.msg = result.execution_result.message if result_status == ExecutionResult.SUCCESS: Logger.localinfo(f" '{self}' : '{self._action_topic}' " - f"detected {len(result.detected_objects)} objects") - if len(result.detected_objects)==0: + f'detected {len(result.detected_objects)} objects') + if len(result.detected_objects) == 0: userdata.items = None - userdata.msg = f"{self.name}: found no objects at location!" + userdata.msg = f"'{self}': found no objects at location!" self._return = 'nothing' else: objects = result.detected_objects @@ -112,14 +124,14 @@ def execute(self, userdata): userdata.items = names self._return = 'done' else: - Logger.localwarn(f"{self} : '{self._action_topic}' -" - f" failed to detect objects ({result_status}) '{userdata.msg}'") + Logger.localwarn(f"'{self}' : '{self._action_topic}' -" + f" failed to detect objects ({result_status}) '{userdata.msg}'") userdata.items = None self._return = 'failed' elif self._node.get_clock().now().nanoseconds - self._start_time.nanoseconds > self._timeout.nanoseconds: # Failed to return call in timely manner self._return = 'failed' - userdata.msg = f"{self._name}: failed to get objects within timeout!" + userdata.msg = f"'{self}': failed to get objects within timeout!" Logger.localwarn(userdata.msg) # Otherwise check for action status change @@ -130,7 +142,7 @@ def execute(self, userdata): Logger.loginfo(f" '{self}' : '{self._action_topic}' - detection goal was aborted! ") self._return = 'failed' except Exception as exc: # pylint: disable=W0703 - Logger.logerr(f"{self._name}: {self._action_topic} exception {type(exc)} - {str(exc)}") + Logger.logerr(f"'{self}': {self._action_topic} exception {type(exc)} - {str(exc)}") self._return = 'failed' # If the action has not yet finished, None outcome will be returned and the state stays active. @@ -139,6 +151,7 @@ def execute(self, userdata): def on_enter(self, userdata): """Call when state becomes active.""" # make sure to reset the data from prior executions + Logger.localinfo(f" on_enter '{self}' - '{self.path}' ...") self._return = None userdata.items = None self._act_client.remove_result(self._action_topic) # clear any prior result from action server @@ -151,7 +164,7 @@ def on_enter(self, userdata): goal = DetectObjects.Goal() else: goal = DetectObjects.Goal(target_object=self._filter) - self._act_client.send_goal(self._action_topic, goal, wait_duration=self._timeout.nanoseconds*1e-9) + self._act_client.send_goal(self._action_topic, goal, wait_duration=self._timeout.nanoseconds * 1e-9) except Exception as exc: # pylint: disable=W0703 # Since a state failure not necessarily causes a behavior failure, # it is recommended to only print warnings, not errors. @@ -162,7 +175,8 @@ def on_enter(self, userdata): def on_exit(self, userdata): """Call when state is deactivated.""" # Make sure that the action is not running when leaving this state. - # A situation where the action would still be active is for example when the operator manually triggers an outcome. + # A situation where the action would still be active is for example when + # the operator manually triggers an outcome. if self._act_client.is_active(self._action_topic): self._act_client.cancel(self._action_topic) @@ -174,11 +188,13 @@ def on_exit(self, userdata): self._return = 'failed' else: status_string = self._act_client.get_status_string(self._action_topic) - Logger.loginfo(f" '{self}' : '{self._action_topic}' - Requested to cancel an active detection request ({status_string}).") + Logger.loginfo(f" '{self}' : '{self._action_topic}' - Requested to cancel " + f'an active detection request ({status_string}).') # Local message are shown in terminal but not the UI # Generally avoid doing this except when debugging if self._return == 'done': - Logger.localinfo(f'Successfully retrieved detected items.') + Logger.localinfo('Successfully retrieved detected items.') else: Logger.localwarn('Failed to detect items.') + Logger.localinfo(f" on_exit '{self}' - '{self.path}' ...") diff --git a/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/door_action_state.py b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/door_action_state.py index 0fb99df..29db4f6 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/door_action_state.py +++ b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/door_action_state.py @@ -78,6 +78,14 @@ def __init__(self, self._start_time = None self._return = None # Retain return value in case the outcome is blocked by operator + def on_pause(self): + """Execute each time this state is paused.""" + Logger.localinfo(f"on_pause '{self}' - '{self.path}' ...") + + def on_resume(self, userdata): + """Execute each time this state is resumed.""" + Logger.localinfo(f"on_resume '{self}' - '{self.path}' ...") + def execute(self, userdata): """ Call periodically while this state is active. @@ -95,7 +103,7 @@ def execute(self, userdata): userdata.msg = result.message # Output message if status == GoalStatus.STATUS_SUCCEEDED: if result.status == ExecutionResult.SUCCESS: - Logger.localinfo(f"{self} - successfully {self._action_outcome}!") + Logger.localinfo(f"'{self}' - successfully {self._action_outcome}!") self._return = 'done' elif result.status in (ExecutionResult.PRECONDITION_FAILURE, ExecutionResult.PLANNING_FAILURE, @@ -111,7 +119,8 @@ def execute(self, userdata): self._return = 'failed' return self._return else: - Logger.logwarn(f"{self} : '{self._topic}' - command invalid action status to '{self._action_type}' '{result.message}'") + Logger.logwarn(f"'{self}' : '{self._topic}' - command invalid action status" + f" to '{self._action_type}' '{result.message}'") self._return = 'failed' elif self._node.get_clock().now().nanoseconds - self._start_time.nanoseconds > self._timeout.nanoseconds: # Failed to return call in timely manner @@ -132,16 +141,16 @@ def execute(self, userdata): # If the action has not yet finished, None outcome will be returned and the state stays active. return self._return - def on_enter(self, userdata): """Call when state becomes active.""" # make sure to reset the error state since a previous state execution might have failed + Logger.localinfo(f"on_enter '{self}' - '{self.path}' ...") self._return = None self._client.remove_result(self._topic) # clear any prior result from action server # Send the goal. try: - self._client.send_goal(self._topic, self._goal, wait_duration=self._timeout.nanoseconds*1e-9) + self._client.send_goal(self._topic, self._goal, wait_duration=self._timeout.nanoseconds * 1e-9) self._start_time = self._node.get_clock().now() except Exception as exc: # pylint: disable=W0703 # Since a state failure not necessarily causes a behavior failure, @@ -161,9 +170,10 @@ def on_exit(self, userdata): # Local message are shown in terminal but not the UI if self._return == 'done': - Logger.localinfo(f'Successfully completed pick action.') + Logger.localinfo('Successfully completed door action.') else: - Logger.localwarn('Failed to complete pick action.') + Logger.localwarn('Failed to complete door action.') + Logger.localinfo(f"on_exit '{self}' - '{self.path}' ...") # Choosing to remove in on_enter and retain in proxy for now # Either choice can be valid. @@ -171,3 +181,11 @@ def on_exit(self, userdata): # # remove the old result so we are ready for the next time # # and don't prematurely return # self._client.remove_result(self._topic) + + def on_start(self): + """Call when behavior starts.""" + Logger.localinfo(f" on_start '{self}' - '{self.path}' ") + + def on_stop(self): + """Call when behavior stops.""" + Logger.localinfo(f" on_stop '{self}' - '{self.path}'") diff --git a/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/follow_path_state.py b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/follow_path_state.py index 2c06ae5..e92d850 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/follow_path_state.py +++ b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/follow_path_state.py @@ -25,11 +25,9 @@ from pyrobosim_msgs.msg import ExecutionResult, Path - - class FollowPathState(EventState): """ - FlexBE state to request following a path for PyRoboSim robot + FlexBE state to request following a path for PyRoboSim robot. Elements defined here for UI Parameters @@ -76,7 +74,9 @@ def execute(self, userdata): if self._client.has_result(self._topic): # Check if the action has been finished result = self._client.get_result(self._topic, clear=True) - Logger.localinfo(f" '{self}' : '{self._topic}' returned {status} {result}") + Logger.localinfo(f" '{self}' : '{self._topic}' returned {status} " + f'result.status={result.execution_result.status}' + f" '{result.execution_result.message}'") if status == GoalStatus.STATUS_SUCCEEDED: result_status = result.execution_result.status userdata.msg = result.execution_result.message # Output message @@ -84,7 +84,7 @@ def execute(self, userdata): self._return = 'done' else: Logger.localwarn(f"{self} : '{self._topic}' -" - f" failed during follow ({result_status}) '{userdata.msg}'") + f" failed during follow ({result_status}) '{userdata.msg}'") self._return = 'failed' return self._return # Note: Not checking a timeout here it is up to follow capability and/or operator to enforce @@ -101,13 +101,12 @@ def execute(self, userdata): # If the action has not yet finished, None outcome will be returned and the state stays active. return self._return - def on_enter(self, userdata): """Call when state becomes active.""" # make sure to reset the data from prior executions + Logger.localinfo(f"on_enter '{self}' - '{self.path}' ...") self._return = None userdata.msg = '' - self._client.remove_result(self._topic) # clear any prior result from action server if 'path' not in userdata: self._return = 'failed' @@ -125,13 +124,15 @@ def on_enter(self, userdata): Logger.localwarn(userdata.msg) self._return = 'failed' return - Logger.localinfo(f"Send follow path goal with {len(path.poses)} waypoints ...") - self._client.send_goal(self._topic, self._goal, wait_duration=self._server_timeout_sec ) + Logger.localinfo(f'Send follow path goal with {len(path.poses)} waypoints ...') + # Send goal clears prior results + self._client.send_goal(self._topic, self._goal, wait_duration=self._server_timeout_sec) except Exception as exc: # pylint: disable=W0703 # Since a state failure not necessarily causes a behavior failure, # it is recommended to only print warnings, not errors. # Using a linebreak before appending the error log enables the operator to collapse details in the GUI. Logger.logwarn(f"Failed to send the '{self}' command:\n {type(exc)} - {exc}") + self._client.remove_result(self._topic) # clear any prior result from action server self._return = 'failed' def on_exit(self, userdata): @@ -142,20 +143,22 @@ def on_exit(self, userdata): if self._client.is_active(self._topic): self._client.cancel(self._topic) - # Check for action status change - status = self._client.get_status(self._topic) + # Check for action status change (blocking call!) + is_terminal, status = self._client.verify_action_status(self._topic, 0.1) if status == GoalStatus.STATUS_CANCELED: Logger.loginfo(f" '{self}' : '{self._topic}' - request to follow was canceled! ") - self._return = 'failed' else: status_string = self._client.get_status_string(self._topic) - Logger.loginfo(f" '{self}' : '{self._topic}' - Requested to cancel an active follow request ({status_string}).") + Logger.loginfo(f" '{self}' : '{self._topic}' - Requested to cancel an active follow request" + f" ({is_terminal}, '{status_string}').") + self._return = 'failed' # Local message are shown in terminal but not the UI if self._return == 'done': - Logger.localinfo(f'Successfully followed path.') + Logger.localinfo('Successfully followed path.') else: Logger.localwarn('Failed to follow path.') + Logger.localinfo(f"on_exit '{self}' - '{self.path}' ...") # Choosing to remove in on_enter and retain in proxy for now # Either choice can be valid. @@ -163,3 +166,34 @@ def on_exit(self, userdata): # # remove the old result so we are ready for the next time # # and don't prematurely return # self._client.remove_result(self._topic) + + def on_pause(self): + """Execute each time this state is paused.""" + Logger.localinfo(f"on_pause '{self}' - '{self.path}' ...") + if self._client.is_active(self._topic): + self._client.cancel(self._topic) + Logger.localinfo(f"Cancelling active follow action '{self}' when paused ...") + # Check for action status change (blocking call!) + is_terminal, status = self._client.verify_action_status(self._topic, 0.1) + if status == GoalStatus.STATUS_CANCELED: + Logger.loginfo(f" '{self}' : '{self._topic}' - request to follow was canceled! ") + else: + status_string = self._client.get_status_string(self._topic) + Logger.loginfo(f" '{self}' : '{self._topic}' - Requested to cancel an active follow request" + f" ({is_terminal}, '{status_string}').") + self._return = 'failed' + + def on_resume(self, userdata): + """Execute each time this state is resumed.""" + Logger.localinfo(f"on_resume '{self}' - '{self.path}' ...") + if self._return is None: + Logger.localinfo(f"Cannot resume follow action '{self}' - require new plan ...") + self._return = 'failed' + + def on_start(self): + """Call when behavior starts.""" + Logger.localinfo(f" on_start '{self}' - '{self.path}' ") + + def on_stop(self): + """Call when behavior stops.""" + Logger.localinfo(f" on_stop '{self}' - '{self.path}'") diff --git a/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/monitor_battery_state.py b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/monitor_battery_state.py new file mode 100644 index 0000000..4a1e7d8 --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/monitor_battery_state.py @@ -0,0 +1,157 @@ +#!/usr/bin/env python + +# Copyright 2024 Christopher Newport University +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""FlexBE State to monitor PyRoboSim robot battery.""" + +from flexbe_core import EventState, Logger +from flexbe_core.proxy import ProxySubscriberCached +from flexbe_core.proxy.qos import QOS_LOSSY + +from pyrobosim_msgs.msg import RobotState + +from rclpy.duration import Duration +from rclpy.time import Time + + +class MonitorBatteryState(EventState): + """ + FlexBE state to monitor PyRoboSim robot battery. + + Elements defined here for UI + Parameters + -- low_battery_level Low level to generate outcome(default=15.0) + -- high_battery_level High level to generate outcome (default=105.0) + -- state_topic Robot state topic name (default= 'robot/robot_state') + -- timeout Max timeout between messages + + Outputs + <= battery_level Battery level check + <= failed Failed to retrieve state + + User data + #> battery_level float Current battery level + """ + + def __init__(self, + low_battery_level=15.0, + high_battery_level=105.0, # Default to being concerned with low-level only + state_topic='robot/robot_state', + timeout=2.0): + # See example_state.py for basic explanations. + super().__init__(outcomes=['battery_level', 'failed'], + input_keys=[], + output_keys=['battery_level']) + + self._low_battery_level = low_battery_level + self._high_battery_level = high_battery_level + self._state_topic = state_topic + self._timeout = Duration(seconds=timeout) + + self._qos = QOS_LOSSY + self._connected = False + self._return = None # Retain return value in case the outcome is blocked by operator + self._state_sub = None + self._battery_level = None + self._last_msg = None + + def on_start(self): + """Call when behavior starts.""" + Logger.localinfo(f" on_start '{self}' - '{self.path}' : subscribe to '{self._state_topic}' ") + self._connect() + + def on_stop(self): + """Call when behavior stops.""" + if self._connected: + # Unsubscribe topic when behavior stops + Logger.localinfo(f" '{self}' : unsubscribe from '{self._state_topic}' ") + ProxySubscriberCached.unsubscribe_topic(self._state_topic) + self._connected = False + Logger.localinfo(f" on_stop '{self}' - '{self.path}'") + + def on_enter(self, userdata): + """Call when state becomes active.""" + # make sure to reset the data from prior executions + Logger.localinfo(f" on_enter '{self}' - '{self.path}' ...") + self._return = None + self._battery_level = None + self._last_msg = None + if not self._connected: + self._connect() # Retry + if not self._connected: + Logger.localinfo(f"Failed to connect to '{self._state_topic}' ") + self._return = 'failed' + + self._state_sub.remove_last_msg(self._state_topic) # Force us to get a new message + self._msg_time = self._node.get_clock().now() + + def on_exit(self, userdata): + """Call when state deactivates.""" + # make sure to reset the data from prior executions + Logger.localinfo(f" on_exit '{self}' - '{self.path}' ...") + + def on_pause(self): + """Execute each time this state is paused.""" + Logger.localinfo(f"on_pause '{self}' - '{self.path}' ...") + + def on_resume(self, userdata): + """Execute each time this state is resumed.""" + Logger.localinfo(f"on_resume '{self}' - '{self.path}' ...") + + def execute(self, userdata): + """ + Call periodically while this state is active. + + Check if the action has been finished and evaluate the result. + """ + if self._return is not None: + # Return prior outcome in case transition is blocked by autonomy level + return self._return + + if self._state_sub.has_msg(self._state_topic): + last_msg = self._state_sub.get_last_msg(self._state_topic) + if last_msg is not self._last_msg: + # Retain last message in proxy for other uses + self._last_msg = last_msg + self._battery_level = self._last_msg.battery_level + self._msg_time = Time(seconds=last_msg.header.stamp.sec, + nanoseconds=last_msg.header.stamp.nanosec, + clock_type=self._node.get_clock().clock_type) # Assumes messages are consistent! + + elapsed = self._node.get_clock().now() - self._msg_time + if elapsed > self._timeout: + self._return = 'failed' + + if self._battery_level is not None: + if self._battery_level < self._low_battery_level: + Logger.logwarn(f'Low battery level = {self._battery_level:.2f}') + self._return = 'battery_level' + if self._battery_level > self._high_battery_level: + Logger.logwarn(f'High battery level = {self._battery_level:.2f}') + self._return = 'battery_level' + + userdata.battery_level = self._battery_level + + # If the action has not yet finished, None outcome will be returned and the state stays active. + return self._return + + def _connect(self): + """Connect to publisher.""" + try: + self._state_sub = ProxySubscriberCached({self._state_topic: RobotState}, qos=self._qos, inst_id=id(self)) + self._connected = True + return True + except Exception: # pylint: disable=W0703 + return False diff --git a/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/navigate_action_state.py b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/navigate_action_state.py index b435ec6..4e447bf 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/navigate_action_state.py +++ b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/navigate_action_state.py @@ -16,24 +16,20 @@ """FlexBE State to navigate PyRoboSim robot to target location.""" -import math - from action_msgs.msg import GoalStatus -from rclpy.duration import Duration - from flexbe_core import EventState, Logger from flexbe_core.proxy import ProxyActionClient from pyrobosim_msgs.action import ExecuteTaskAction from pyrobosim_msgs.msg import ExecutionResult, TaskAction - +from rclpy.duration import Duration class NavigateActionState(EventState): """ - FlexBE state to navigate PyRoboSim robot to target location + FlexBE state to navigate PyRoboSim robot to target location. Elements defined here for UI Parameters @@ -52,7 +48,7 @@ class NavigateActionState(EventState): #> msg Output message """ - def __init__(self, target_location, robot_name="robot", + def __init__(self, target_location, robot_name='robot', action_topic='/execute_action', server_timeout=5.0, navigate_timeout=None): # See example_state.py for basic explanations. @@ -62,9 +58,9 @@ def __init__(self, target_location, robot_name="robot", self._goal = ExecuteTaskAction.Goal() self._goal.action = TaskAction(robot=robot_name, - type="navigate", + type='navigate', target_location=target_location, - ) + ) self._timeout = None if navigate_timeout is not None: @@ -97,7 +93,7 @@ def execute(self, userdata): Logger.localinfo(f" '{self}' : '{self._topic}' returned {status} {result}") if status == GoalStatus.STATUS_SUCCEEDED: result = result.execution_result - self._elapsed = (self._node.get_clock().now() - self._start_time).nanoseconds*1e-9 + self._elapsed = (self._node.get_clock().now() - self._start_time).nanoseconds * 1e-9 userdata.msg = result.message if result.status == ExecutionResult.SUCCESS: @@ -134,10 +130,10 @@ def execute(self, userdata): # If the action has not yet finished, None outcome will be returned and the state stays active. return self._return - def on_enter(self, userdata): """Call when state becomes active.""" # make sure to reset the return state since a previous state execution might have failed + Logger.localinfo(f"on_enter '{self}' - '{self.path}' ...") self._return = None self._client.remove_result(self._topic) # clear any prior result from action server @@ -169,6 +165,7 @@ def on_exit(self, userdata): Logger.loginfo(f'Successfully completed motion in {self._elapsed:.3f} seconds.') else: Logger.logwarn('Failed to complete motion.') + Logger.localinfo(f"on_exit '{self}' - '{self.path}' ...") # Choosing to remove in on_enter and retain in proxy for now # Either choice can be valid. @@ -176,3 +173,19 @@ def on_exit(self, userdata): # # remove the old result so we are ready for the next time # # and don't prematurely return # self._client.remove_result(self._topic) + + def on_pause(self): + """Execute each time this state is paused.""" + Logger.localinfo(f"on_pause '{self}' - '{self.path}' ...") + + def on_resume(self, userdata): + """Execute each time this state is resumed.""" + Logger.localinfo(f"on_resume '{self}' - '{self.path}' ...") + + def on_start(self): + """Call when behavior starts.""" + Logger.localinfo(f" on_start '{self}' - '{self.path}' ") + + def on_stop(self): + """Call when behavior stops.""" + Logger.localinfo(f" on_stop '{self}' - '{self.path}'") diff --git a/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/next_room_state.py b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/next_room_state.py new file mode 100644 index 0000000..d4fc543 --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/next_room_state.py @@ -0,0 +1,187 @@ +#!/usr/bin/env python + +# Copyright 2024 Christopher Newport University +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""FlexBE State to determine the next room along the way to goal in PyRoboSim world.""" + +from flexbe_core import EventState, Logger +from flexbe_core.proxy import ProxySubscriberCached +from flexbe_core.proxy.qos import QOS_LOSSY + +from geometry_msgs.msg import Pose + +from pyrobosim_flexbe_utilities.world_configuration import WorldConfiguration + +from pyrobosim_msgs.msg import RobotState + +from rclpy.duration import Duration +from rclpy.time import Time + + +class NextRoomState(EventState): + """ + FlexBE state to determine next room to move given pyrobosim world definition. + + Elements defined here for UI + Parameters + -- world_definition world definition (default='world4') + -- package_name package name (default='delib_ws_worlds') + -- sub_folder Subfolder for world definition(default='worlds') + -- state_topic Robot state topic name (default= 'robot/robot_state') + -- timeout Max timeout between messages + + Outputs + <= done Next room goal is set + <= failed Failed to retrieve goal + + User data + ># goal string where we want to go overall + #> room string where we need to go next + #> hallway string connecting hallway (door) + """ + + def __init__(self, + world='world4', + package_name='delib_ws_worlds', + sub_folder='worlds', + state_topic='robot/robot_state', + timeout=2.0): + # See example_state.py for basic explanations. + super().__init__(outcomes=['done', 'failed'], + input_keys=['goal'], + output_keys=['room', 'hallway']) + + self._state_topic = state_topic + self._timeout = Duration(seconds=timeout) + self._qos = QOS_LOSSY + self._connected = False + self._return = None # Retain return value in case the outcome is blocked by operator + self._state_sub = None + self._last_msg = None + self._last_visited = None + self._last_pose = None + + self._world = world + + # Load structure of world as needed + self._world_config = WorldConfiguration(world, package_name, sub_folder) + + def on_start(self): + """Call when behavior starts.""" + Logger.localinfo(f"on_start '{self}' - '{self.path}' : subscribe to '{self._state_topic}' ") + self._connect() + + def on_stop(self): + """Call when behavior stops.""" + if self._connected: + # Unsubscribe topic when behavior stops + Logger.localinfo(f" '{self}' : unsubscribe from '{self._state_topic}' ") + ProxySubscriberCached.unsubscribe_topic(self._state_topic) + self._connected = False + Logger.localinfo(f"on_stop '{self}' - '{self.path}' ...") + + def on_enter(self, userdata): + """Call when state becomes active.""" + # make sure to reset the data from prior executions + Logger.localinfo(f"on_enter '{self}' - '{self.path}' ...") + + self._return = None + self._last_msg = None + self._last_visited = None + self._last_pose = None + + if not self._connected: + self._connect() # Retry + if not self._connected: + Logger.localinfo(f"Failed to connect to '{self._state_topic}' ") + self._return = 'failed' + + self._state_sub.remove_last_msg(self._state_topic) # Force us to get a new message + self._msg_time = self._node.get_clock().now() + + def on_exit(self, userdata): + """Execute when state is deactivated.""" + Logger.localinfo(f"on_exit '{self}' - '{self.path}' ...") + + def on_pause(self): + """Execute each time this state is paused.""" + Logger.localinfo(f"on_pause '{self}' - '{self.path}' ...") + + def on_resume(self, userdata): + """Execute each time this state is resumed.""" + Logger.localinfo(f"on_resume '{self}' - '{self.path}' ...") + + def execute(self, userdata): + """ + Call periodically while this state is active. + + Check if the action has been finished and evaluate the result. + """ + if self._return is not None: + # Return prior outcome in case transition is blocked by autonomy level + return self._return + + if self._state_sub.has_msg(self._state_topic): + last_msg = self._state_sub.get_last_msg(self._state_topic) + self._last_visited = last_msg.last_visited_location + self._last_pose = last_msg.pose + self._msg_time = Time(seconds=last_msg.header.stamp.sec, + nanoseconds=last_msg.header.stamp.nanosec, + clock_type=self._node.get_clock().clock_type) # Assumes messages are consistent! + + elapsed = self._node.get_clock().now() - self._msg_time + if elapsed > self._timeout: + Logger.logerr('Failed to get robot state message for last visited') + self._return = 'failed' + return self._return + + if self._last_visited is None: + return None + + try: + # For now hope we get lucky choosing index [0] if multiple rooms returned + target_room = self._world_config.get_rooms(userdata.goal, self._world)[0] + current_room = self._world_config.get_rooms(self._last_visited, self._world, self._last_pose)[0] + + path = self._world_config.search(current_room, target_room, self._world) + if path is not None and len(path) > 1: + userdata.room = path[1] # next room ([0] is current) + userdata.hallway = self._world_config.get_hallway(current_room, path[1], self._world) + Logger.loginfo(f" travel '{current_room}' to '{userdata.room}' via '{userdata.hallway}'") + if userdata.room == 'dining': + userdata.room = Pose() + userdata.room.position.x = -1.0 # a valid pose to left of dining table + Logger.localinfo(f'Hack: Dining is not reachable so replace with reachable pose {userdata.room}!') + self._return = 'done' + else: + Logger.logerr(f"Failed to find valid path from '{current_room}' to '{target_room}'") + self._return = 'failed' + + except Exception as exc: + Logger.logerr(f'Error processing room request:\n {exc}') + self._return = 'failed' + + # If the action has not yet finished, None outcome will be returned and the state stays active + # while we wait on another robot state message. + return self._return + + def _connect(self): + """Connect to publisher.""" + try: + self._state_sub = ProxySubscriberCached({self._state_topic: RobotState}, qos=self._qos, inst_id=id(self)) + self._connected = True + return True + except Exception: # pylint: disable=W0703 + return False diff --git a/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/pick_action_state.py b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/pick_action_state.py index 294964f..2b06ac8 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/pick_action_state.py +++ b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/pick_action_state.py @@ -29,7 +29,7 @@ class PickActionState(EventState): """ - FlexBE state to pick action for PyRoboSim robot + FlexBE state to pick action for PyRoboSim robot. Elements defined here for UI Parameters @@ -46,7 +46,7 @@ class PickActionState(EventState): #> msg string Result message """ - def __init__(self, robot_name="robot", + def __init__(self, robot_name='robot', action_topic='/execute_action', timeout=2.0): # See example_state.py for basic explanations. @@ -55,7 +55,7 @@ def __init__(self, robot_name="robot", output_keys=['msg']) self._goal = ExecuteTaskAction.Goal() - self._goal.action = TaskAction(robot=robot_name, type="pick") + self._goal.action = TaskAction(robot=robot_name, type='pick') self._topic = action_topic self._timeout = Duration(seconds=timeout) @@ -99,12 +99,12 @@ def execute(self, userdata): self._return = 'failed' return self._return else: - Logger.logwarn(f"{self} : '{self._topic}' - invalid action status '{result.message}'") + Logger.logwarn(f"'{self}' : '{self._topic}' - invalid action status '{result.message}'") self._return = 'failed' elif self._node.get_clock().now().nanoseconds - self._start_time.nanoseconds > self._timeout.nanoseconds: # Failed to return call in timely manner self._return = 'failed' - userdata.msg = f"{self._name}: failed to pick object within timeout!" + userdata.msg = f"'{self}': failed to pick object within timeout!" Logger.localwarn(userdata.msg) # Otherwise check for status change @@ -118,10 +118,10 @@ def execute(self, userdata): # If the action has not yet finished, None outcome will be returned and the state stays active. return self._return - def on_enter(self, userdata): """Call when state becomes active.""" # make sure to reset the error state since a previous state execution might have failed + Logger.localinfo(f"on_enter '{self}' - '{self.path}' ...") self._return = None self._client.remove_result(self._topic) # clear any prior result from action server @@ -134,7 +134,7 @@ def on_enter(self, userdata): # Send the goal. try: self._goal.action.object = userdata.object - self._client.send_goal(self._topic, self._goal, wait_duration=self._timeout.nanoseconds*1e-9) + self._client.send_goal(self._topic, self._goal, wait_duration=self._timeout.nanoseconds * 1e-9) self._start_time = self._node.get_clock().now() except Exception as exc: # pylint: disable=W0703 # Since a state failure not necessarily causes a behavior failure, @@ -154,9 +154,10 @@ def on_exit(self, userdata): # Local message are shown in terminal but not the UI if self._return == 'done': - Logger.localinfo(f'Successfully completed pick action.') + Logger.localinfo('Successfully completed pick action.') else: Logger.localwarn('Failed to complete pick action.') + Logger.localinfo(f"on_exit '{self}' - '{self.path}' ...") # Choosing to remove in on_enter and retain in proxy for now # Either choice can be valid. @@ -164,3 +165,19 @@ def on_exit(self, userdata): # # remove the old result so we are ready for the next time # # and don't prematurely return # self._client.remove_result(self._topic) + + def on_pause(self): + """Execute each time this state is paused.""" + Logger.localinfo(f"on_pause '{self}' - '{self.path}' ...") + + def on_resume(self, userdata): + """Execute each time this state is resumed.""" + Logger.localinfo(f"on_resume '{self}' - '{self.path}' ...") + + def on_start(self): + """Call when behavior starts.""" + Logger.localinfo(f" on_start '{self}' - '{self.path}' ") + + def on_stop(self): + """Call when behavior stops.""" + Logger.localinfo(f" on_stop '{self}' - '{self.path}'") diff --git a/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/place_action_state.py b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/place_action_state.py index ca7e82a..c376b95 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/place_action_state.py +++ b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/place_action_state.py @@ -29,7 +29,7 @@ class PlaceActionState(EventState): """ - FlexBE state to place object action for PyRoboSim robot + FlexBE state to place object action for PyRoboSim robot. Elements defined here for UI Parameters @@ -55,8 +55,7 @@ def __init__(self, robot_name='robot', self._goal = ExecuteTaskAction.Goal() self._goal.action = TaskAction(robot=robot_name, - type="place", - ) + type='place') self._topic = action_topic self._timeout = Duration(seconds=timeout) @@ -84,18 +83,18 @@ def execute(self, userdata): userdata.msg = result.message # Output message if status == GoalStatus.STATUS_SUCCEEDED: if result.status == ExecutionResult.SUCCESS: - Logger.localinfo(f"{self} - successfully placed object") + Logger.localinfo(f"'{self}' - successfully placed object") self._return = 'done' elif result.status in (ExecutionResult.PRECONDITION_FAILURE, ExecutionResult.PLANNING_FAILURE, ExecutionResult.EXECUTION_FAILURE): - Logger.logwarn(f"{self} : '{self._topic}' - place failure '{result.message}'") + Logger.logwarn(f"'{self}' : '{self._topic}' - place failure '{result.message}'") self._return = 'failed' elif result.status in (ExecutionResult.CANCELED): - Logger.logwarn(f"{self} : '{self._topic}' - canceled '{result.message}'") + Logger.logwarn(f"'{self}' : '{self._topic}' - canceled '{result.message}'") self._return = 'failed' else: - Logger.logwarn(f"{self} : '{self._topic}' -" + Logger.logwarn(f"'{self}' : '{self._topic}' -" f" unknown failure ({result.status}) '{result.message}'") self._return = 'failed' return self._return @@ -105,7 +104,7 @@ def execute(self, userdata): elif self._node.get_clock().now().nanoseconds - self._start_time.nanoseconds > self._timeout.nanoseconds: # Failed to return call in timely manner self._return = 'failed' - userdata.msg = f"{self._name}: failed to place object within timeout!" + userdata.msg = f"'{self._name}': failed to place object within timeout!" Logger.localwarn(userdata.msg) # Otherwise check for status change @@ -119,16 +118,16 @@ def execute(self, userdata): # If the action has not yet finished, None outcome will be returned and the state stays active. return self._return - def on_enter(self, userdata): """Call when state becomes active.""" # make sure to reset the error state since a previous state execution might have failed + Logger.localinfo(f"on_enter '{self}' - '{self.path}' ...") self._return = None self._client.remove_result(self._topic) # clear any prior result from action server # Send the goal. try: - self._client.send_goal(self._topic, self._goal, wait_duration=self._timeout.nanoseconds*1e-9) + self._client.send_goal(self._topic, self._goal, wait_duration=self._timeout.nanoseconds * 1e-9) self._start_time = self._node.get_clock().now() except Exception as exc: # pylint: disable=W0703 # Since a state failure not necessarily causes a behavior failure, @@ -148,9 +147,10 @@ def on_exit(self, userdata): # Local message are shown in terminal but not the UI if self._return == 'done': - Logger.localinfo(f'Successfully completed place action.') + Logger.localinfo('Successfully completed place action.') else: Logger.localwarn('Failed to complete place action.') + Logger.localinfo(f"on_exit '{self}' - '{self.path}' ...") # Choosing to remove in on_enter and retain in proxy for now # Either choice can be valid. @@ -158,3 +158,19 @@ def on_exit(self, userdata): # # remove the old result so we are ready for the next time # # and don't prematurely return # self._client.remove_result(self._topic) + + def on_pause(self): + """Execute each time this state is paused.""" + Logger.localinfo(f"on_pause '{self}' - '{self.path}' ...") + + def on_resume(self, userdata): + """Execute each time this state is resumed.""" + Logger.localinfo(f"on_resume '{self}' - '{self.path}' ...") + + def on_start(self): + """Call when behavior starts.""" + Logger.localinfo(f" on_start '{self}' - '{self.path}' ") + + def on_stop(self): + """Call when behavior stops.""" + Logger.localinfo(f" on_stop '{self}' - '{self.path}'") diff --git a/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/plan_path_state.py b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/plan_path_state.py index 5228225..876e543 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/plan_path_state.py +++ b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/plan_path_state.py @@ -31,12 +31,12 @@ class PlanPathState(EventState): """ - FlexBE state to request plan for PyRoboSim robot + FlexBE state to request plan for PyRoboSim robot. Elements defined here for UI Parameters -- action_topic Action topic name (default= 'robot/plan_path') - -- timeout Total time to wait for plan in seconds (default = 5s) + -- timeout Total time to wait for plan in seconds (default = 10s) Outputs <= done Successfully planned path to goal. @@ -49,7 +49,7 @@ class PlanPathState(EventState): """ def __init__(self, action_topic='robot/plan_path', - timeout=2.0): + timeout=10.0): # See example_state.py for basic explanations. super().__init__(outcomes=['done', 'failed'], input_keys=['goal'], @@ -83,13 +83,17 @@ def execute(self, userdata): if status == GoalStatus.STATUS_SUCCEEDED: result_status = result.execution_result.status userdata.path = None + loc = self._goal.target_location + if loc == '': + loc = self._goal.target_pose # Allow for using Pose instead of string location name if result_status == ExecutionResult.SUCCESS and result.path is not None: - Logger.localinfo(f"{self} - planning success with {len(result.path.poses)} waypoints") + Logger.localinfo(f"'{self}' - planning to target location='{loc}' " + f'successful with {len(result.path.poses)} waypoints') userdata.path = result.path self._return = 'done' else: - Logger.localwarn(f"{self} : '{self._topic}' -" - f" planning failure ({result_status}) '{userdata.msg}'") + Logger.localwarn(f"'{self}' : '{self._topic}' - target location='{loc}'" + f" planning failure ({result_status}) '{userdata.msg}'") self._return = 'failed' return self._return else: @@ -98,7 +102,7 @@ def execute(self, userdata): elif self._node.get_clock().now().nanoseconds - self._start_time.nanoseconds > self._planning_timeout.nanoseconds: # Failed to return call in timely manner self._return = 'failed' - userdata.msg = f"{self._name}: failed to plan path within timeout!" + userdata.msg = f"'{self}': failed to plan path within timeout!" Logger.localwarn(userdata.msg) # Otherwise check for action status change @@ -112,14 +116,13 @@ def execute(self, userdata): # If the action has not yet finished, None outcome will be returned and the state stays active. return self._return - def on_enter(self, userdata): """Call when state becomes active.""" # make sure to reset the data from prior executions + Logger.localinfo(f"on_enter '{self}' - '{self.path}' ...") self._return = None userdata.path = None userdata.msg = '' - self._client.remove_result(self._topic) # clear any prior result from action server if 'goal' not in userdata: self._return = 'failed' userdata.msg = f"PlanActionState '{self}' requires userdata.goal key!" @@ -130,15 +133,17 @@ def on_enter(self, userdata): try: goal = userdata.goal if isinstance(goal, str): - self._goal = PlanPath.Goal(target_location = goal) + self._goal = PlanPath.Goal(target_location=goal) elif isinstance(goal, Pose): - self._goal = PlanPath.Goal(pose=goal) + self._goal = PlanPath.Goal(target_pose=goal) + Logger.localinfo(f'Using {self._goal.target_pose} as goal pose!') else: userdata.msg = f"Invalid goal type '{type(goal)}' - must be string or Pose!" Logger.localwarn(userdata.msg) self._return = 'failed' return - self._client.send_goal(self._topic, self._goal, wait_duration=self._planning_timeout.nanoseconds*1e-9) + # Send goal clears the prior results + self._client.send_goal(self._topic, self._goal, wait_duration=self._planning_timeout.nanoseconds * 1e-9) self._start_time = self._node.get_clock().now() except Exception as exc: # pylint: disable=W0703 # Since a state failure not necessarily causes a behavior failure, @@ -146,6 +151,7 @@ def on_enter(self, userdata): # Using a linebreak before appending the error log enables the operator to collapse details in the GUI. Logger.logwarn(f"Failed to send the '{self}' command:\n {type(exc)} - {exc}") self._return = 'failed' + self._client.remove_result(self._topic) # clear any prior result from action server def on_exit(self, userdata): """Call when state is deactivated.""" @@ -154,20 +160,22 @@ def on_exit(self, userdata): if self._client.is_active(self._topic): self._client.cancel(self._topic) - # Check for action status change - status = self._client.get_status(self._topic) # get status before clearing result + # Check for action status change (blocking call!) + is_terminal, status = self._client.verify_action_status(self._topic, 0.1) if status == GoalStatus.STATUS_CANCELED: Logger.loginfo(f" '{self}' : '{self._topic}' - request to plan was canceled! ") - self._return = 'failed' else: status_string = self._client.get_status_string(self._topic) - Logger.loginfo(f" '{self}' : '{self._topic}' - Requested to cancel an active plan request ({status_string}).") + Logger.loginfo(f" '{self}' : '{self._topic}' - Requested to cancel an active plan request" + f" ({is_terminal}, '{status_string}').") + self._return = 'failed' # Local message are shown in terminal but not the UI if self._return == 'done': - Logger.localinfo(f'Successfully planned path.') + Logger.localinfo('Successfully planned path.') else: Logger.localwarn('Failed to plan path.') + Logger.localinfo(f"on_exit '{self}' - '{self.path}' ...") # Choosing to remove in on_enter and retain in proxy for now # Either choice can be valid. @@ -175,3 +183,34 @@ def on_exit(self, userdata): # # remove the old result so we are ready for the next time # # and don't prematurely return # self._client.remove_result(self._topic) + + def on_pause(self): + """Execute each time this state is paused.""" + Logger.localinfo(f"on_pause '{self}' - '{self.path}' ...") + if self._client.is_active(self._topic): + self._client.cancel(self._topic) + Logger.localinfo(f"Cancelling active planning request '{self}' when paused ...") + # Check for action status change (blocking call!) + is_terminal, status = self._client.verify_action_status(self._topic, 0.1) + if status == GoalStatus.STATUS_CANCELED: + Logger.loginfo(f" '{self}' : '{self._topic}' - request to plan was canceled! ") + else: + status_string = self._client.get_status_string(self._topic) + Logger.loginfo(f" '{self}' : '{self._topic}' - Requested to cancel an active plan request" + f" ({is_terminal}, '{status_string}').") + self._return = 'failed' + + def on_resume(self, userdata): + """Execute each time this state is resumed.""" + Logger.localinfo(f"on_resume '{self}' - '{self.path}' ...") + if self._return is None: + Logger.localinfo(f"Cannot resume planning action '{self}' - require new request ...") + self._return = 'failed' + + def on_start(self): + """Call when behavior starts.""" + Logger.localinfo(f" on_start '{self}' - '{self.path}' ") + + def on_stop(self): + """Call when behavior stops.""" + Logger.localinfo(f" on_stop '{self}' - '{self.path}'") diff --git a/technologies/FlexBE/pyrobosim_flexbe_states/setup.py b/technologies/FlexBE/pyrobosim_flexbe_states/setup.py index 751d489..6f93354 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_states/setup.py +++ b/technologies/FlexBE/pyrobosim_flexbe_states/setup.py @@ -1,8 +1,9 @@ #!/usr/bin/env python from glob import glob -from setuptools import setup + from setuptools import find_packages +from setuptools import setup PACKAGE_NAME = 'pyrobosim_flexbe_states' @@ -11,24 +12,21 @@ version='0.0.1', packages=find_packages(), data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + PACKAGE_NAME]), + ('share/ament_index/resource_index/packages', ['resource/' + PACKAGE_NAME]), ('share/' + PACKAGE_NAME, ['package.xml']), - ('share/' + PACKAGE_NAME + "/tests", glob('tests/*.test')), - ('share/' + PACKAGE_NAME + "/launch", glob('tests/*.launch.py')), + ('share/' + PACKAGE_NAME + '/tests', glob('tests/*.test')), + ('share/' + PACKAGE_NAME + '/launch', glob('tests/*.launch.py')), ], install_requires=['setuptools'], zip_safe=True, - maintainer='TODO', - maintainer_email='TODO@TODO.com', - description='TODO: Package description', - license='TODO: License declaration', + maintainer='David Conner', + maintainer_email='robotics@cnu.edu', + description='Interface FlexBE state implementations for Pyrobosim', + license='Apache 2.0', tests_require=['pytest'], entry_points={ 'console_scripts': [ - 'example_action_state = pyrobosim_flexbe_states.example_action_state', - 'example_state = pyrobosim_flexbe_states.example_state', ], }, ) diff --git a/technologies/FlexBE/pyrobosim_flexbe_states/tests/launch_test.py b/technologies/FlexBE/pyrobosim_flexbe_states/tests/launch_test.py index 1a9bc72..b17c9fa 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_states/tests/launch_test.py +++ b/technologies/FlexBE/pyrobosim_flexbe_states/tests/launch_test.py @@ -30,11 +30,12 @@ from os.path import join +from ament_index_python.packages import get_package_share_directory + from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription -from launch.substitutions import LaunchConfiguration from launch.launch_description_sources import PythonLaunchDescriptionSource -from ament_index_python.packages import get_package_share_directory +from launch.substitutions import LaunchConfiguration def generate_launch_description(): @@ -42,23 +43,23 @@ def generate_launch_description(): flexbe_testing_dir = get_package_share_directory('flexbe_testing') flexbe_states_test_dir = get_package_share_directory('pyrobosim_flexbe_states') - path = join(flexbe_states_test_dir, "tests") + path = join(flexbe_states_test_dir, 'tests') # The tests - testcases = "" - testcases += join(path, "example_state.test") + "\n" - testcases += join(path, "example_action_state.test") + "\n" + testcases = '' + testcases += join(path, 'example_state.test') + '\n' + testcases += join(path, 'example_action_state.test') + '\n' return LaunchDescription([ - DeclareLaunchArgument("pkg", default_value="pyrobosim_flexbe_states"), - DeclareLaunchArgument("testcases", default_value=testcases), - DeclareLaunchArgument("compact_format", default_value='true'), + DeclareLaunchArgument('pkg', default_value='pyrobosim_flexbe_states'), + DeclareLaunchArgument('testcases', default_value=testcases), + DeclareLaunchArgument('compact_format', default_value='true'), IncludeLaunchDescription( - PythonLaunchDescriptionSource(join(flexbe_testing_dir, "launch", "flexbe_testing.launch.py")), + PythonLaunchDescriptionSource(join(flexbe_testing_dir, 'launch', 'flexbe_testing.launch.py')), launch_arguments={ - 'package': LaunchConfiguration("pkg"), - 'compact_format': LaunchConfiguration("compact_format"), - 'testcases': LaunchConfiguration("testcases"), + 'package': LaunchConfiguration('pkg'), + 'compact_format': LaunchConfiguration('compact_format'), + 'testcases': LaunchConfiguration('testcases'), }.items() ) ]) diff --git a/technologies/FlexBE/pyrobosim_flexbe_states/tests/run_colcon_test.py b/technologies/FlexBE/pyrobosim_flexbe_states/tests/run_colcon_test.py index 9b20f19..be46c28 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_states/tests/run_colcon_test.py +++ b/technologies/FlexBE/pyrobosim_flexbe_states/tests/run_colcon_test.py @@ -42,9 +42,9 @@ class TestFlexBEStates(PyTester): @classmethod def setUpClass(cls): - - PyTester._package = "pyrobosim_flexbe_states" - PyTester._tests_folder = "tests" + """Set up the test class.""" + PyTester._package = 'pyrobosim_flexbe_states' + PyTester._tests_folder = 'tests' PyTester.setUpClass() # Do this last after setting package and tests folder diff --git a/technologies/FlexBE/pyrobosim_flexbe_utilities/package.xml b/technologies/FlexBE/pyrobosim_flexbe_utilities/package.xml new file mode 100644 index 0000000..d1fd175 --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_utilities/package.xml @@ -0,0 +1,34 @@ + + + + pyrobosim_flexbe_utilities + 0.0.1 + + Pyrobosim utilities. + + Implemented for ROSCon '24 Deliberative Technologies Workshop. + + + David Conner + David Conner + Apache 2 + + rclpy + flexbe_core + pyrobosim_ros + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + ament_cmake + ament_cmake_python + + + ament_python + + + diff --git a/technologies/FlexBE/pyrobosim_flexbe_utilities/pyrobosim_flexbe_utilities/__init__.py b/technologies/FlexBE/pyrobosim_flexbe_utilities/pyrobosim_flexbe_utilities/__init__.py new file mode 100644 index 0000000..8c4ef15 --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_utilities/pyrobosim_flexbe_utilities/__init__.py @@ -0,0 +1 @@ +"""pyrobosim_flexbe_utilities package.""" diff --git a/technologies/FlexBE/pyrobosim_flexbe_utilities/pyrobosim_flexbe_utilities/world_configuration.py b/technologies/FlexBE/pyrobosim_flexbe_utilities/pyrobosim_flexbe_utilities/world_configuration.py new file mode 100644 index 0000000..54cf1f8 --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_utilities/pyrobosim_flexbe_utilities/world_configuration.py @@ -0,0 +1,202 @@ +#!/usr/bin/env python + +# Copyright 2024 Christopher Newport University +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""FlexBE utility for loading PyRoboSim world structure.""" + +import os +import re +from collections import deque + +from ament_index_python.packages import get_package_share_directory + +from shapely.geometry import Point, Polygon + +import yaml + + +def signed_distance_to_polygon(polygon, point): + """ + Calculate the signed distance to polygon boundary. + + Use negative value for points inside the polygon. + """ + # Calculate the distance from the point to the polygon boundary + distance = point.distance(polygon) + + # Check if the point is inside the polygon + if polygon.contains(point): + return -distance # Negative if inside + else: + return distance # Positive if outside + + +class WorldConfiguration: + """ + FlexBE utility to load and allow queries regarding the pyrobosim world definition. + + Parameters + ---------- + -- world_definition world definition (default='world4') + -- package_name package name (default='delib_ws_worlds') + -- sub_folder subfolder for world definition(default='worlds') + -- state_topic robot state topic name (default= 'robot/robot_state') + + """ + + __world_data = {} # World data shared by all instances + + def __init__(self, + world='world4', + package_name='delib_ws_worlds', + sub_folder='worlds'): + + if world not in WorldConfiguration.__world_data: + WorldConfiguration._load_world_data(world, package_name, sub_folder) + else: + print(f"'{world}' is already initialized!") + + def get_rooms(self, value, world, pose=None): + """Return tuple of possible rooms.""" + if world not in WorldConfiguration.__world_data: + raise KeyError(f"'{world}' has not been initialized") + + world_data = WorldConfiguration.__world_data[world] + + if value.startswith('hall_'): + # This is a door between two rooms + possible_rooms = tuple(value[len('hall_'):].split('_')) + print(f'hallway between rooms {possible_rooms}') + if pose is not None: + distances = [signed_distance_to_polygon(world_data['rooms'][room], + Point(pose.position.x, pose.position.y)) for room in possible_rooms] + possible_rooms = (possible_rooms[0], ) if distances[0] < distances[1] else (possible_rooms[1], ) + print(f' choosing {possible_rooms} based on pose!') + return possible_rooms + + if value in world_data['hallway_graph']: + return (value,) # is a room + + endings = ['_dock', '_tabletop', '_disposal', '_storage'] + locations = [(next((value.replace(ending, '') for ending in endings if value.endswith(ending)), value))] + + category = re.sub(r'\d+$', '', value) # strip trailing quantifier (e.g. 'bread2') + if category in world_data['categories']: + # Assumes category and location are unique + locations = world_data['categories'][category] + print(f" '{value}' can be found in '{locations}'") + + rooms = [world_data['locations'][loc] for loc in locations if loc in world_data['locations']] + if len(rooms) > 0: + print(f" '{value}' is in '{rooms}'") + return tuple(rooms) + + raise ValueError(f"'{value}' cannot be found in '{world}'") + + def get_hallway(self, current_room, next_room, world): + """Return hallway name that connects two rooms.""" + return WorldConfiguration.__world_data[world]['hallway_graph'][current_room][next_room] + + def search(self, start, goal, world): + """Do breadth first search which finds optimal for constant edge costs.""" + if world not in WorldConfiguration.__world_data: + raise KeyError(f"'{world}' has not been initialized") + + graph = WorldConfiguration.__world_data[world]['hallway_graph'] + + if start not in graph or goal not in graph: + print(f"Invalid rooms for search from '{start}' to '{goal}' in '{world}'") + return None + + # Queue for BFS (assumes small graph) + queue = deque([start]) + # Dictionary to track distances and the shortest path + distances = {start: 0} + previous_nodes = {start: None} + + while queue: + current_node = queue.popleft() + + # If we reached the goal, construct the path + if current_node == goal: + path = [] + while current_node is not None: + path.insert(0, current_node) + current_node = previous_nodes[current_node] + print(f"Found path from '{start}' to '{goal}' : {path}") + return path + + # Explore neighbors + for neighbor in graph[current_node]: + if neighbor not in distances: + queue.append(neighbor) + distances[neighbor] = distances[current_node] + 1 + previous_nodes[neighbor] = current_node + + return None # No path found + + @classmethod + def _load_world_data(cls, world, package_name, sub_folder): + """ + Load world data from pyrobosim world definition. + + Use class data so that multiple states only need to load one shared set of data. + """ + # If this data does not exist, the behavior will throw exception on construction! + package_path = get_package_share_directory(package_name) + world_file_path = os.path.join(package_path, sub_folder, world + '.yaml') + print(f"Attempting to load '{world_file_path}' ...", flush=True) + with open(world_file_path) as fin: + world_data = yaml.safe_load(fin) + + hallways = world_data['hallways'] + hallway_graph = {} + for hallway in hallways: + room_start = hallway['room_start'] + room_end = hallway['room_end'] + hallway_name = f'hall_{room_start}_{room_end}' + # Add the edge in both directions since the hallways are bidirectional + if room_start not in hallway_graph: + hallway_graph[room_start] = {} + if room_end not in hallway_graph: + hallway_graph[room_end] = {} + + hallway_graph[room_start][room_end] = hallway_name + hallway_graph[room_end][room_start] = hallway_name + + locations = {} + for location in world_data['locations']: + locations[location['name']] = location['parent'] # in room + + categories = {} + for obj in world_data['objects']: + category = obj['category'] + if category not in categories: + categories[category] = [] + categories[category].append(obj['parent']) # possible locations + + rooms = {} + for room in world_data['rooms']: + if room['footprint']['type'] != 'polygon': + print(f"Invalid footprint type '{room['footprint']['type']}' for '{room['name']}'") + continue + rooms[room['name']] = Polygon(room['footprint']['coords']) + + print(f'Loaded {len(categories)} categories in {len(locations)} locations' + f' in {len(hallway_graph)} ({len(rooms)}) rooms') + WorldConfiguration.__world_data[world] = {'hallway_graph': hallway_graph, + 'locations': locations, + 'categories': categories, + 'rooms': rooms} diff --git a/technologies/FlexBE/pyrobosim_flexbe_utilities/resource/pyrobosim_flexbe_utilities b/technologies/FlexBE/pyrobosim_flexbe_utilities/resource/pyrobosim_flexbe_utilities new file mode 100644 index 0000000..e69de29 diff --git a/technologies/FlexBE/pyrobosim_flexbe_utilities/setup.cfg b/technologies/FlexBE/pyrobosim_flexbe_utilities/setup.cfg new file mode 100644 index 0000000..936c22f --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_utilities/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/pyrobosim_flexbe_utilities +[install] +install_scripts=$base/lib/pyrobosim_flexbe_utilities diff --git a/technologies/FlexBE/pyrobosim_flexbe_utilities/setup.py b/technologies/FlexBE/pyrobosim_flexbe_utilities/setup.py new file mode 100644 index 0000000..b947c90 --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_utilities/setup.py @@ -0,0 +1,30 @@ +#!/usr/bin/env python + +from glob import glob + +from setuptools import find_packages +from setuptools import setup + +PACKAGE_NAME = 'pyrobosim_flexbe_utilities' + +setup( + name=PACKAGE_NAME, + version='0.0.1', + packages=find_packages(), + data_files=[ + ('share/ament_index/resource_index/packages', ['resource/' + PACKAGE_NAME]), + ('share/' + PACKAGE_NAME, ['package.xml']), + ], + + install_requires=['setuptools'], + zip_safe=True, + maintainer='David Conner', + maintainer_email='robotics@cnu.edu', + description='Pyrobosim utilities used by pyrobosim_flexbe_states ', + license='Apache 2.0', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + ], + }, +) From f0aca0d6533c61cd4f4cfe158d11aa9b2349e715 Mon Sep 17 00:00:00 2001 From: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Date: Tue, 1 Oct 2024 18:37:03 -0400 Subject: [PATCH 43/51] Set ROS_LOCALHOST_ONLY and do not run entrypoint twice on container startup (#50) --- .docker/Dockerfile | 1 - .docker/entrypoint.sh | 1 - docker-compose.yaml | 4 ++++ 3 files changed, 4 insertions(+), 2 deletions(-) diff --git a/.docker/Dockerfile b/.docker/Dockerfile index 9bb4daa..fcf2887 100644 --- a/.docker/Dockerfile +++ b/.docker/Dockerfile @@ -57,5 +57,4 @@ ENV NO_AT_BRIDGE 1 # Set up the entrypoint. CMD /bin/bash COPY .docker/entrypoint.sh /entrypoint.sh -ENTRYPOINT ["/entrypoint.sh"] RUN echo "source /entrypoint.sh" >> ~/.bashrc diff --git a/.docker/entrypoint.sh b/.docker/entrypoint.sh index 6ee9f80..80c5b71 100755 --- a/.docker/entrypoint.sh +++ b/.docker/entrypoint.sh @@ -8,7 +8,6 @@ export PYTHONWARNINGS="ignore:setup.py install is deprecated,ignore:easy_install export ROS_WS=/delib_ws export WORKSPACE_ROOT=$ROS_WS # For FlexBE compatibility -export ROS_DOMAIN_ID=0 function print_ros_variables () { echo -e "ROS Distro: \t" $ROS_DISTRO diff --git a/docker-compose.yaml b/docker-compose.yaml index eb9f051..3d6a49a 100644 --- a/docker-compose.yaml +++ b/docker-compose.yaml @@ -30,6 +30,10 @@ services: # Needed to run Groot inside the container privileged: true environment: + # Ensure your ROS 2 system is only visible on the local network + - ROS_AUTOMATIC_DISCOVERY_RANGE=LOCALHOST + # If you disable ROS_LOCALHOST_ONLY above, ensure you use a unique domain ID on your network + - ROS_DOMAIN_ID=0 # Allows graphical programs in the container - DISPLAY=${DISPLAY} - QT_X11_NO_MITSHM=1 From a56fe183f40118bc0d4966c696868734bcce582c Mon Sep 17 00:00:00 2001 From: David Conner Date: Wed, 2 Oct 2024 20:18:53 -0400 Subject: [PATCH 44/51] merged PR #41 to allow testing of ros_bt_py --- dependencies/ros_bt_py/ros2_ros_bt_py | 2 +- technologies/ros_bt_py/README.md | 91 ++ .../launch/ros_bt_py_pyrobosim.launch.py | 25 + .../ros_bt_py/ros_bt_py_pyrobosim/package.xml | 21 + .../resource/ros_bt_py_pyrobosim | 0 .../ros_bt_py_pyrobosim/__init__.py | 0 .../ros_bt_py_pyrobosim/nodes/__init__.py | 2 + .../ros_bt_py_pyrobosim/nodes/actions.py | 11 + .../ros_bt_py_pyrobosim/nodes/services.py | 88 ++ .../ros_bt_py/ros_bt_py_pyrobosim/setup.cfg | 4 + .../ros_bt_py/ros_bt_py_pyrobosim/setup.py | 29 + .../trees/move_to_pose.yaml | 566 +++++++++++ .../ros_bt_py_pyrobosim/trees/open_door.yaml | 215 ++++ .../ros_bt_py_pyrobosim/trees/pick_up.yaml | 286 ++++++ .../ros_bt_py_pyrobosim/trees/plan_path.yaml | 391 +++++++ .../ros_bt_py_pyrobosim/trees/problem_1.yaml | 908 +++++++++++++++++ .../ros_bt_py_pyrobosim/trees/problem_2.yaml | 957 ++++++++++++++++++ .../ros_bt_py_pyrobosim/trees/put_down.yaml | 286 ++++++ 18 files changed, 3881 insertions(+), 1 deletion(-) create mode 100644 technologies/ros_bt_py/README.md create mode 100644 technologies/ros_bt_py/ros_bt_py_pyrobosim/launch/ros_bt_py_pyrobosim.launch.py create mode 100644 technologies/ros_bt_py/ros_bt_py_pyrobosim/package.xml create mode 100644 technologies/ros_bt_py/ros_bt_py_pyrobosim/resource/ros_bt_py_pyrobosim create mode 100644 technologies/ros_bt_py/ros_bt_py_pyrobosim/ros_bt_py_pyrobosim/__init__.py create mode 100644 technologies/ros_bt_py/ros_bt_py_pyrobosim/ros_bt_py_pyrobosim/nodes/__init__.py create mode 100644 technologies/ros_bt_py/ros_bt_py_pyrobosim/ros_bt_py_pyrobosim/nodes/actions.py create mode 100644 technologies/ros_bt_py/ros_bt_py_pyrobosim/ros_bt_py_pyrobosim/nodes/services.py create mode 100644 technologies/ros_bt_py/ros_bt_py_pyrobosim/setup.cfg create mode 100644 technologies/ros_bt_py/ros_bt_py_pyrobosim/setup.py create mode 100644 technologies/ros_bt_py/ros_bt_py_pyrobosim/trees/move_to_pose.yaml create mode 100644 technologies/ros_bt_py/ros_bt_py_pyrobosim/trees/open_door.yaml create mode 100644 technologies/ros_bt_py/ros_bt_py_pyrobosim/trees/pick_up.yaml create mode 100644 technologies/ros_bt_py/ros_bt_py_pyrobosim/trees/plan_path.yaml create mode 100644 technologies/ros_bt_py/ros_bt_py_pyrobosim/trees/problem_1.yaml create mode 100644 technologies/ros_bt_py/ros_bt_py_pyrobosim/trees/problem_2.yaml create mode 100644 technologies/ros_bt_py/ros_bt_py_pyrobosim/trees/put_down.yaml diff --git a/dependencies/ros_bt_py/ros2_ros_bt_py b/dependencies/ros_bt_py/ros2_ros_bt_py index 60cfd02..0fef933 160000 --- a/dependencies/ros_bt_py/ros2_ros_bt_py +++ b/dependencies/ros_bt_py/ros2_ros_bt_py @@ -1 +1 @@ -Subproject commit 60cfd02a38b67c84f48b01fc6b8f28f8b9ac819b +Subproject commit 0fef933fe03174cd836b24ffe6ee228a0481e050 diff --git a/technologies/ros_bt_py/README.md b/technologies/ros_bt_py/README.md new file mode 100644 index 0000000..d760840 --- /dev/null +++ b/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. + diff --git a/technologies/ros_bt_py/ros_bt_py_pyrobosim/launch/ros_bt_py_pyrobosim.launch.py b/technologies/ros_bt_py/ros_bt_py_pyrobosim/launch/ros_bt_py_pyrobosim.launch.py new file mode 100644 index 0000000..b84147c --- /dev/null +++ b/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]) + diff --git a/technologies/ros_bt_py/ros_bt_py_pyrobosim/package.xml b/technologies/ros_bt_py/ros_bt_py_pyrobosim/package.xml new file mode 100644 index 0000000..7ee271b --- /dev/null +++ b/technologies/ros_bt_py/ros_bt_py_pyrobosim/package.xml @@ -0,0 +1,21 @@ + + + + ros_bt_py_pyrobosim + 1.0.0 + BT nodes for the ROSCon2024 workshop + David Oberacker + TODO: License declaration + + pyrobosim_msgs + ros_bt_py + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/technologies/ros_bt_py/ros_bt_py_pyrobosim/resource/ros_bt_py_pyrobosim b/technologies/ros_bt_py/ros_bt_py_pyrobosim/resource/ros_bt_py_pyrobosim new file mode 100644 index 0000000..e69de29 diff --git a/technologies/ros_bt_py/ros_bt_py_pyrobosim/ros_bt_py_pyrobosim/__init__.py b/technologies/ros_bt_py/ros_bt_py_pyrobosim/ros_bt_py_pyrobosim/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/technologies/ros_bt_py/ros_bt_py_pyrobosim/ros_bt_py_pyrobosim/nodes/__init__.py b/technologies/ros_bt_py/ros_bt_py_pyrobosim/ros_bt_py_pyrobosim/nodes/__init__.py new file mode 100644 index 0000000..cf850b5 --- /dev/null +++ b/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 diff --git a/technologies/ros_bt_py/ros_bt_py_pyrobosim/ros_bt_py_pyrobosim/nodes/actions.py b/technologies/ros_bt_py/ros_bt_py_pyrobosim/ros_bt_py_pyrobosim/nodes/actions.py new file mode 100644 index 0000000..4ee4254 --- /dev/null +++ b/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 + diff --git a/technologies/ros_bt_py/ros_bt_py_pyrobosim/ros_bt_py_pyrobosim/nodes/services.py b/technologies/ros_bt_py/ros_bt_py_pyrobosim/ros_bt_py_pyrobosim/nodes/services.py new file mode 100644 index 0000000..94383bd --- /dev/null +++ b/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 diff --git a/technologies/ros_bt_py/ros_bt_py_pyrobosim/setup.cfg b/technologies/ros_bt_py/ros_bt_py_pyrobosim/setup.cfg new file mode 100644 index 0000000..d0073d1 --- /dev/null +++ b/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 diff --git a/technologies/ros_bt_py/ros_bt_py_pyrobosim/setup.py b/technologies/ros_bt_py/ros_bt_py_pyrobosim/setup.py new file mode 100644 index 0000000..ab6dabb --- /dev/null +++ b/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='oberacker@fzi.de', + description='BT nodes for the ROSCon2024 workshop', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + ], + }, +) diff --git a/technologies/ros_bt_py/ros_bt_py_pyrobosim/trees/move_to_pose.yaml b/technologies/ros_bt_py/ros_bt_py_pyrobosim/trees/move_to_pose.yaml new file mode 100644 index 0000000..42fe91d --- /dev/null +++ b/technologies/ros_bt_py/ros_bt_py_pyrobosim/trees/move_to_pose.yaml @@ -0,0 +1,566 @@ +name: '' +path: '' +root_name: RootSequence +nodes: + - module: ros_bt_py.nodes.sequence + node_class: MemorySequence + version: 0.1.0 + max_children: -1 + name: RootSequence + child_names: + - SetupSequence + - PlanPathSequence + - FollowPathSequence + options: [] + inputs: [] + outputs: [] + state: SHUTDOWN + - module: ros_bt_py.nodes.sequence + node_class: MemorySequence + version: 0.1.0 + max_children: -1 + name: SetupSequence + child_names: + - TargetPoseInput + - EnumFields + - ZeroFloat + - OneFloat + - ZeroQuaternion + - ZeroPoint + - CreateZeroPose + options: [] + inputs: [] + outputs: [] + state: SHUTDOWN + - module: ros_bt_py.nodes.io + node_class: IOInputOption + version: 0.1.0 + max_children: 0 + name: TargetPoseInput + child_names: [] + options: + - key: io_type + serialized_value: '{"py/type": "builtins.str"}' + serialized_type: '{"py/type": "builtins.type"}' + - key: default + serialized_value: '"kitchen"' + serialized_type: '{"py/type": "builtins.str"}' + inputs: + - key: in + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + outputs: + - key: out + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + state: SHUTDOWN + - module: ros_bt_py.ros_nodes.enum + node_class: EnumFields + version: 0.1.0 + max_children: 0 + name: EnumFields + child_names: [] + options: + - key: ros_message_type + serialized_value: '{"py/type": "pyrobosim_msgs.msg._execution_result.ExecutionResult"}' + serialized_type: '{"py/type": "builtins.type"}' + inputs: [] + outputs: + - key: CANCELED + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: EXECUTION_FAILURE + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: INVALID_ACTION + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: PLANNING_FAILURE + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: POSTCONDITION_FAILURE + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: PRECONDITION_FAILURE + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: STATUS__DEFAULT + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: SUCCESS + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: UNKNOWN + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + state: SHUTDOWN + - module: ros_bt_py.nodes.constant + node_class: Constant + version: 0.1.0 + max_children: 0 + name: ZeroFloat + child_names: [] + options: + - key: constant_type + serialized_value: '{"py/type": "builtins.float"}' + serialized_type: '{"py/type": "builtins.type"}' + - key: constant_value + serialized_value: '0.0' + serialized_type: '{"py/type": "builtins.float"}' + inputs: [] + outputs: + - key: constant + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.float"}' + state: SHUTDOWN + - module: ros_bt_py.nodes.constant + node_class: Constant + version: 0.1.0 + max_children: 0 + name: OneFloat + child_names: [] + options: + - key: constant_type + serialized_value: '{"py/type": "builtins.float"}' + serialized_type: '{"py/type": "builtins.type"}' + - key: constant_value + serialized_value: '1.0' + serialized_type: '{"py/type": "builtins.float"}' + inputs: [] + outputs: + - key: constant + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.float"}' + state: SHUTDOWN + - module: ros_bt_py.ros_nodes.message_converters + node_class: FieldsToMessage + version: 0.1.0 + max_children: 0 + name: ZeroQuaternion + child_names: [] + options: + - key: output_type + serialized_value: '{"py/type": "geometry_msgs.msg._quaternion.Quaternion"}' + serialized_type: '{"py/type": "builtins.type"}' + inputs: + - key: x + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.float"}' + - key: 'y' + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.float"}' + - key: z + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.float"}' + - key: w + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.float"}' + outputs: + - key: out + serialized_value: 'null' + serialized_type: '{"py/type": "geometry_msgs.msg._quaternion.Quaternion"}' + state: SHUTDOWN + - module: ros_bt_py.ros_nodes.message_converters + node_class: FieldsToMessage + version: 0.1.0 + max_children: 0 + name: ZeroPoint + child_names: [] + options: + - key: output_type + serialized_value: '{"py/type": "geometry_msgs.msg._point.Point"}' + serialized_type: '{"py/type": "builtins.type"}' + inputs: + - key: x + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.float"}' + - key: 'y' + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.float"}' + - key: z + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.float"}' + outputs: + - key: out + serialized_value: 'null' + serialized_type: '{"py/type": "geometry_msgs.msg._point.Point"}' + state: SHUTDOWN + - module: ros_bt_py.ros_nodes.message_converters + node_class: FieldsToMessage + version: 0.1.0 + max_children: 0 + name: CreateZeroPose + child_names: [] + options: + - key: output_type + serialized_value: '{"py/type": "geometry_msgs.msg._pose.Pose"}' + serialized_type: '{"py/type": "builtins.type"}' + inputs: + - key: position + serialized_value: 'null' + serialized_type: '{"py/type": "geometry_msgs.msg._point.Point"}' + - key: orientation + serialized_value: 'null' + serialized_type: '{"py/type": "geometry_msgs.msg._quaternion.Quaternion"}' + outputs: + - key: out + serialized_value: 'null' + serialized_type: '{"py/type": "geometry_msgs.msg._pose.Pose"}' + state: SHUTDOWN + - module: ros_bt_py.nodes.sequence + node_class: MemorySequence + version: 0.1.0 + max_children: -1 + name: PlanPathSequence + child_names: + - PlanPathAction + - GetPlanPathExecutionResult + - PlanPathSuccessul? + options: [] + inputs: [] + outputs: [] + state: SHUTDOWN + - module: ros_bt_py.ros_nodes.action + node_class: Action + version: 0.1.0 + max_children: 0 + name: PlanPathAction + child_names: [] + options: + - key: action_type + serialized_value: '{"py/type": "pyrobosim_msgs.action._plan_path.PlanPath"}' + serialized_type: '{"py/type": "builtins.type"}' + - key: action_name + serialized_value: '"/robot/plan_path"' + serialized_type: '{"py/type": "builtins.str"}' + - key: wait_for_action_server_seconds + serialized_value: '2.0' + serialized_type: '{"py/type": "builtins.float"}' + - key: timeout_seconds + serialized_value: '5.0' + serialized_type: '{"py/type": "builtins.float"}' + - key: fail_if_not_available + serialized_value: 'true' + serialized_type: '{"py/type": "builtins.bool"}' + inputs: + - key: target_location + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + - key: target_pose + serialized_value: 'null' + serialized_type: '{"py/type": "geometry_msgs.msg._pose.Pose"}' + outputs: + - key: result_execution_result + serialized_value: 'null' + serialized_type: '{"py/type": "pyrobosim_msgs.msg._execution_result.ExecutionResult"}' + - key: result_path + serialized_value: 'null' + serialized_type: '{"py/type": "pyrobosim_msgs.msg._path.Path"}' + state: SHUTDOWN + - module: ros_bt_py.ros_nodes.message_converters + node_class: MessageToFields + version: 0.1.0 + max_children: 0 + name: GetPlanPathExecutionResult + child_names: [] + options: + - key: input_type + serialized_value: '{"py/type": "pyrobosim_msgs.msg._execution_result.ExecutionResult"}' + serialized_type: '{"py/type": "builtins.type"}' + inputs: + - key: in + serialized_value: 'null' + serialized_type: '{"py/type": "pyrobosim_msgs.msg._execution_result.ExecutionResult"}' + outputs: + - key: status + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: message + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + state: SHUTDOWN + - module: ros_bt_py.nodes.compare + node_class: Compare + version: 0.1.0 + max_children: 0 + name: PlanPathSuccessul? + child_names: [] + options: + - key: compare_type + serialized_value: '{"py/type": "builtins.int"}' + serialized_type: '{"py/type": "builtins.type"}' + inputs: + - key: a + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: b + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + outputs: [] + state: SHUTDOWN + - module: ros_bt_py.nodes.sequence + node_class: MemorySequence + version: 0.1.0 + max_children: -1 + name: FollowPathSequence + child_names: + - FollowPathAction + - GetFollowPathExecutionResult + - FollowPathSuccessfull? + options: [] + inputs: [] + outputs: [] + state: SHUTDOWN + - module: ros_bt_py.ros_nodes.action + node_class: Action + version: 0.1.0 + max_children: 0 + name: FollowPathAction + child_names: [] + options: + - key: action_type + serialized_value: '{"py/type": "pyrobosim_msgs.action._follow_path.FollowPath"}' + serialized_type: '{"py/type": "builtins.type"}' + - key: action_name + serialized_value: '"/robot/follow_path"' + serialized_type: '{"py/type": "builtins.str"}' + - key: wait_for_action_server_seconds + serialized_value: '5.0' + serialized_type: '{"py/type": "builtins.float"}' + - key: timeout_seconds + serialized_value: '30.0' + serialized_type: '{"py/type": "builtins.float"}' + - key: fail_if_not_available + serialized_value: 'true' + serialized_type: '{"py/type": "builtins.bool"}' + inputs: + - key: path + serialized_value: 'null' + serialized_type: '{"py/type": "pyrobosim_msgs.msg._path.Path"}' + outputs: + - key: result_execution_result + serialized_value: 'null' + serialized_type: '{"py/type": "pyrobosim_msgs.msg._execution_result.ExecutionResult"}' + state: SHUTDOWN + - module: ros_bt_py.ros_nodes.message_converters + node_class: MessageToFields + version: 0.1.0 + max_children: 0 + name: GetFollowPathExecutionResult + child_names: [] + options: + - key: input_type + serialized_value: '{"py/type": "pyrobosim_msgs.msg._execution_result.ExecutionResult"}' + serialized_type: '{"py/type": "builtins.type"}' + inputs: + - key: in + serialized_value: 'null' + serialized_type: '{"py/type": "pyrobosim_msgs.msg._execution_result.ExecutionResult"}' + outputs: + - key: status + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: message + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + state: SHUTDOWN + - module: ros_bt_py.nodes.compare + node_class: Compare + version: 0.1.0 + max_children: 0 + name: FollowPathSuccessfull? + child_names: [] + options: + - key: compare_type + serialized_value: '{"py/type": "builtins.int"}' + serialized_type: '{"py/type": "builtins.type"}' + inputs: + - key: a + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: b + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + outputs: [] + state: SHUTDOWN +data_wirings: + - source: + node_name: PlanPathAction + data_kind: outputs + data_key: result_path + target: + node_name: FollowPathAction + data_kind: inputs + data_key: path + - source: + node_name: EnumFields + data_kind: outputs + data_key: SUCCESS + target: + node_name: PlanPathSuccessul? + data_kind: inputs + data_key: b + - source: + node_name: PlanPathAction + data_kind: outputs + data_key: result_execution_result + target: + node_name: GetPlanPathExecutionResult + data_kind: inputs + data_key: in + - source: + node_name: GetPlanPathExecutionResult + data_kind: outputs + data_key: status + target: + node_name: PlanPathSuccessul? + data_kind: inputs + data_key: a + - source: + node_name: FollowPathAction + data_kind: outputs + data_key: result_execution_result + target: + node_name: GetFollowPathExecutionResult + data_kind: inputs + data_key: in + - source: + node_name: GetFollowPathExecutionResult + data_kind: outputs + data_key: status + target: + node_name: FollowPathSuccessfull? + data_kind: inputs + data_key: a + - source: + node_name: EnumFields + data_kind: outputs + data_key: SUCCESS + target: + node_name: FollowPathSuccessfull? + data_kind: inputs + data_key: b + - source: + node_name: TargetPoseInput + data_kind: outputs + data_key: out + target: + node_name: PlanPathAction + data_kind: inputs + data_key: target_location + - source: + node_name: CreateZeroPose + data_kind: outputs + data_key: out + target: + node_name: PlanPathAction + data_kind: inputs + data_key: target_pose + - source: + node_name: ZeroPoint + data_kind: outputs + data_key: out + target: + node_name: CreateZeroPose + data_kind: inputs + data_key: position + - source: + node_name: ZeroQuaternion + data_kind: outputs + data_key: out + target: + node_name: CreateZeroPose + data_kind: inputs + data_key: orientation + - source: + node_name: OneFloat + data_kind: outputs + data_key: constant + target: + node_name: ZeroQuaternion + data_kind: inputs + data_key: w + - source: + node_name: ZeroFloat + data_kind: outputs + data_key: constant + target: + node_name: ZeroQuaternion + data_kind: inputs + data_key: x + - source: + node_name: ZeroFloat + data_kind: outputs + data_key: constant + target: + node_name: ZeroQuaternion + data_kind: inputs + data_key: 'y' + - source: + node_name: ZeroFloat + data_kind: outputs + data_key: constant + target: + node_name: ZeroQuaternion + data_kind: inputs + data_key: z + - source: + node_name: ZeroFloat + data_kind: outputs + data_key: constant + target: + node_name: ZeroPoint + data_kind: inputs + data_key: x + - source: + node_name: ZeroFloat + data_kind: outputs + data_key: constant + target: + node_name: ZeroPoint + data_kind: inputs + data_key: 'y' + - source: + node_name: ZeroFloat + data_kind: outputs + data_key: constant + target: + node_name: ZeroPoint + data_kind: inputs + data_key: z +tick_frequency_hz: 10 +state: EDITABLE +public_node_data: + - node_name: TargetPoseInput + data_kind: inputs + data_key: in + - node_name: EnumFields + data_kind: outputs + data_key: CANCELED + - node_name: EnumFields + data_kind: outputs + data_key: EXECUTION_FAILURE + - node_name: EnumFields + data_kind: outputs + data_key: INVALID_ACTION + - node_name: EnumFields + data_kind: outputs + data_key: PLANNING_FAILURE + - node_name: EnumFields + data_kind: outputs + data_key: POSTCONDITION_FAILURE + - node_name: EnumFields + data_kind: outputs + data_key: PRECONDITION_FAILURE + - node_name: EnumFields + data_kind: outputs + data_key: STATUS__DEFAULT + - node_name: EnumFields + data_kind: outputs + data_key: UNKNOWN + - node_name: GetPlanPathExecutionResult + data_kind: outputs + data_key: message + - node_name: GetFollowPathExecutionResult + data_kind: outputs + data_key: message diff --git a/technologies/ros_bt_py/ros_bt_py_pyrobosim/trees/open_door.yaml b/technologies/ros_bt_py/ros_bt_py_pyrobosim/trees/open_door.yaml new file mode 100644 index 0000000..8448168 --- /dev/null +++ b/technologies/ros_bt_py/ros_bt_py_pyrobosim/trees/open_door.yaml @@ -0,0 +1,215 @@ +name: '' +path: '' +root_name: .OpenDoorSequence +nodes: + - module: ros_bt_py.nodes.sequence + node_class: MemorySequence + version: 0.1.0 + max_children: -1 + name: OpenDoorSequence + child_names: + - EnumExeuctionResult + - OpenDoorTask + - OpenDoorAction + - GetOpenDoorResult + - OpenDoorSuccessfull? + options: [] + inputs: [] + outputs: [] + state: SHUTDOWN + - module: ros_bt_py.ros_nodes.enum + node_class: EnumFields + version: 0.1.0 + max_children: 0 + name: EnumExeuctionResult + child_names: [] + options: + - key: ros_message_type + serialized_value: '{"py/type": "pyrobosim_msgs.msg._execution_result.ExecutionResult"}' + serialized_type: '{"py/type": "builtins.type"}' + inputs: [] + outputs: + - key: CANCELED + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: EXECUTION_FAILURE + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: INVALID_ACTION + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: PLANNING_FAILURE + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: POSTCONDITION_FAILURE + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: PRECONDITION_FAILURE + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: STATUS__DEFAULT + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: SUCCESS + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: UNKNOWN + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + state: SHUTDOWN + - module: ros_bt_py.ros_nodes.messages_from_dict + node_class: MessageFromConstDict + version: 0.9.0 + max_children: 0 + name: OpenDoorTask + child_names: [] + options: + - key: message_type + serialized_value: '{"py/type": "pyrobosim_msgs.msg._task_action.TaskAction"}' + serialized_type: '{"py/type": "builtins.type"}' + - key: dict + serialized_value: '{"robot": "robot", "type": "open"}' + serialized_type: '{"py/type": "builtins.dict"}' + inputs: [] + outputs: + - key: message + serialized_value: 'null' + serialized_type: '{"py/type": "pyrobosim_msgs.msg._task_action.TaskAction"}' + state: SHUTDOWN + - module: ros_bt_py.ros_nodes.action + node_class: Action + version: 0.1.0 + max_children: 0 + name: OpenDoorAction + child_names: [] + options: + - key: action_type + serialized_value: >- + {"py/type": + "pyrobosim_msgs.action._execute_task_action.ExecuteTaskAction"} + serialized_type: '{"py/type": "builtins.type"}' + - key: action_name + serialized_value: '"/execute_action"' + serialized_type: '{"py/type": "builtins.str"}' + - key: wait_for_action_server_seconds + serialized_value: '1.2' + serialized_type: '{"py/type": "builtins.float"}' + - key: timeout_seconds + serialized_value: '5.0' + serialized_type: '{"py/type": "builtins.float"}' + - key: fail_if_not_available + serialized_value: 'true' + serialized_type: '{"py/type": "builtins.bool"}' + inputs: + - key: action + serialized_value: 'null' + serialized_type: '{"py/type": "pyrobosim_msgs.msg._task_action.TaskAction"}' + outputs: + - key: result_execution_result + serialized_value: 'null' + serialized_type: '{"py/type": "pyrobosim_msgs.msg._execution_result.ExecutionResult"}' + state: SHUTDOWN + - module: ros_bt_py.ros_nodes.message_converters + node_class: MessageToFields + version: 0.1.0 + max_children: 0 + name: GetOpenDoorResult + child_names: [] + options: + - key: input_type + serialized_value: '{"py/type": "pyrobosim_msgs.msg._execution_result.ExecutionResult"}' + serialized_type: '{"py/type": "builtins.type"}' + inputs: + - key: in + serialized_value: 'null' + serialized_type: '{"py/type": "pyrobosim_msgs.msg._execution_result.ExecutionResult"}' + outputs: + - key: status + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: message + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + state: SHUTDOWN + - module: ros_bt_py.nodes.compare + node_class: Compare + version: 0.1.0 + max_children: 0 + name: OpenDoorSuccessfull? + child_names: [] + options: + - key: compare_type + serialized_value: '{"py/type": "builtins.int"}' + serialized_type: '{"py/type": "builtins.type"}' + inputs: + - key: a + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: b + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + outputs: [] + state: SHUTDOWN +data_wirings: + - source: + node_name: OpenDoorTask + data_kind: outputs + data_key: message + target: + node_name: OpenDoorAction + data_kind: inputs + data_key: action + - source: + node_name: OpenDoorAction + data_kind: outputs + data_key: result_execution_result + target: + node_name: GetOpenDoorResult + data_kind: inputs + data_key: in + - source: + node_name: EnumExeuctionResult + data_kind: outputs + data_key: SUCCESS + target: + node_name: OpenDoorSuccessfull? + data_kind: inputs + data_key: a + - source: + node_name: GetOpenDoorResult + data_kind: outputs + data_key: status + target: + node_name: OpenDoorSuccessfull? + data_kind: inputs + data_key: b +tick_frequency_hz: 10 +state: EDITABLE +public_node_data: + - node_name: EnumExeuctionResult + data_kind: outputs + data_key: CANCELED + - node_name: EnumExeuctionResult + data_kind: outputs + data_key: EXECUTION_FAILURE + - node_name: EnumExeuctionResult + data_kind: outputs + data_key: INVALID_ACTION + - node_name: EnumExeuctionResult + data_kind: outputs + data_key: PLANNING_FAILURE + - node_name: EnumExeuctionResult + data_kind: outputs + data_key: POSTCONDITION_FAILURE + - node_name: EnumExeuctionResult + data_kind: outputs + data_key: PRECONDITION_FAILURE + - node_name: EnumExeuctionResult + data_kind: outputs + data_key: STATUS__DEFAULT + - node_name: EnumExeuctionResult + data_kind: outputs + data_key: UNKNOWN + - node_name: GetOpenDoorResult + data_kind: outputs + data_key: message diff --git a/technologies/ros_bt_py/ros_bt_py_pyrobosim/trees/pick_up.yaml b/technologies/ros_bt_py/ros_bt_py_pyrobosim/trees/pick_up.yaml new file mode 100644 index 0000000..e99a11a --- /dev/null +++ b/technologies/ros_bt_py/ros_bt_py_pyrobosim/trees/pick_up.yaml @@ -0,0 +1,286 @@ +name: pick_up.yaml +path: '' +root_name: .TempPickUpSequence +nodes: + - module: ros_bt_py.nodes.sequence + node_class: MemorySequence + version: 0.1.0 + max_children: -1 + name: PickUpSequence + child_names: + - ObjectToPickUp + - EnumExeuctionResult + - PickUpRequest + - SetAttr_2 + - PickUpObject + - PickUpResult + - PickUpSuccessfull? + options: [] + inputs: [] + outputs: [] + state: SHUTDOWN + - module: ros_bt_py.nodes.io + node_class: IOInputOption + version: 0.1.0 + max_children: 0 + name: ObjectToPickUp + child_names: [] + options: + - key: io_type + serialized_value: '{"py/type": "builtins.str"}' + serialized_type: '{"py/type": "builtins.type"}' + - key: default + serialized_value: '"object"' + serialized_type: '{"py/type": "builtins.str"}' + inputs: + - key: in + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + outputs: + - key: out + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + state: SHUTDOWN + - module: ros_bt_py.ros_nodes.enum + node_class: EnumFields + version: 0.1.0 + max_children: 0 + name: EnumExeuctionResult + child_names: [] + options: + - key: ros_message_type + serialized_value: '{"py/type": "pyrobosim_msgs.msg._execution_result.ExecutionResult"}' + serialized_type: '{"py/type": "builtins.type"}' + inputs: [] + outputs: + - key: CANCELED + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: EXECUTION_FAILURE + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: INVALID_ACTION + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: PLANNING_FAILURE + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: POSTCONDITION_FAILURE + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: PRECONDITION_FAILURE + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: STATUS__DEFAULT + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: SUCCESS + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: UNKNOWN + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + state: SHUTDOWN + - module: ros_bt_py.ros_nodes.messages_from_dict + node_class: MessageFromConstDict + version: 0.9.0 + max_children: 0 + name: PickUpRequest + child_names: [] + options: + - key: message_type + serialized_value: '{"py/type": "pyrobosim_msgs.msg._task_action.TaskAction"}' + serialized_type: '{"py/type": "builtins.type"}' + - key: dict + serialized_value: '{"robot": "robot", "type": "pick"}' + serialized_type: '{"py/type": "builtins.dict"}' + inputs: [] + outputs: + - key: message + serialized_value: 'null' + serialized_type: '{"py/type": "pyrobosim_msgs.msg._task_action.TaskAction"}' + state: SHUTDOWN + - module: ros_bt_py.nodes.setters + node_class: SetAttr + version: 0.1.0 + max_children: 0 + name: SetAttr_2 + child_names: [] + options: + - key: object_type + serialized_value: '{"py/type": "pyrobosim_msgs.msg._task_action.TaskAction"}' + serialized_type: '{"py/type": "builtins.type"}' + - key: attr_name + serialized_value: '"object"' + serialized_type: '{"py/type": "builtins.str"}' + - key: attr_type + serialized_value: '{"py/type": "builtins.str"}' + serialized_type: '{"py/type": "builtins.type"}' + inputs: + - key: object + serialized_value: 'null' + serialized_type: '{"py/type": "pyrobosim_msgs.msg._task_action.TaskAction"}' + - key: attr_value + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + outputs: + - key: new_object + serialized_value: 'null' + serialized_type: '{"py/type": "pyrobosim_msgs.msg._task_action.TaskAction"}' + state: SHUTDOWN + - module: ros_bt_py.ros_nodes.action + node_class: Action + version: 0.1.0 + max_children: 0 + name: PickUpObject + child_names: [] + options: + - key: action_type + serialized_value: >- + {"py/type": + "pyrobosim_msgs.action._execute_task_action.ExecuteTaskAction"} + serialized_type: '{"py/type": "builtins.type"}' + - key: action_name + serialized_value: '"/execute_action"' + serialized_type: '{"py/type": "builtins.str"}' + - key: wait_for_action_server_seconds + serialized_value: '2.0' + serialized_type: '{"py/type": "builtins.float"}' + - key: timeout_seconds + serialized_value: '5.0' + serialized_type: '{"py/type": "builtins.float"}' + - key: fail_if_not_available + serialized_value: 'true' + serialized_type: '{"py/type": "builtins.bool"}' + inputs: + - key: action + serialized_value: 'null' + serialized_type: '{"py/type": "pyrobosim_msgs.msg._task_action.TaskAction"}' + outputs: + - key: result_execution_result + serialized_value: 'null' + serialized_type: '{"py/type": "pyrobosim_msgs.msg._execution_result.ExecutionResult"}' + state: SHUTDOWN + - module: ros_bt_py.ros_nodes.message_converters + node_class: MessageToFields + version: 0.1.0 + max_children: 0 + name: PickUpResult + child_names: [] + options: + - key: input_type + serialized_value: '{"py/type": "pyrobosim_msgs.msg._execution_result.ExecutionResult"}' + serialized_type: '{"py/type": "builtins.type"}' + inputs: + - key: in + serialized_value: 'null' + serialized_type: '{"py/type": "pyrobosim_msgs.msg._execution_result.ExecutionResult"}' + outputs: + - key: status + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: message + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + state: SHUTDOWN + - module: ros_bt_py.nodes.compare + node_class: Compare + version: 0.1.0 + max_children: 0 + name: PickUpSuccessfull? + child_names: [] + options: + - key: compare_type + serialized_value: '{"py/type": "builtins.int"}' + serialized_type: '{"py/type": "builtins.type"}' + inputs: + - key: a + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: b + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + outputs: [] + state: SHUTDOWN +data_wirings: + - source: + node_name: ObjectToPickUp + data_kind: outputs + data_key: out + target: + node_name: SetAttr_2 + data_kind: inputs + data_key: attr_value + - source: + node_name: PickUpRequest + data_kind: outputs + data_key: message + target: + node_name: SetAttr_2 + data_kind: inputs + data_key: object + - source: + node_name: SetAttr_2 + data_kind: outputs + data_key: new_object + target: + node_name: PickUpObject + data_kind: inputs + data_key: action + - source: + node_name: PickUpObject + data_kind: outputs + data_key: result_execution_result + target: + node_name: PickUpResult + data_kind: inputs + data_key: in + - source: + node_name: EnumExeuctionResult + data_kind: outputs + data_key: SUCCESS + target: + node_name: PickUpSuccessfull? + data_kind: inputs + data_key: a + - source: + node_name: PickUpResult + data_kind: outputs + data_key: status + target: + node_name: PickUpSuccessfull? + data_kind: inputs + data_key: b +tick_frequency_hz: 10 +state: EDITABLE +public_node_data: + - node_name: ObjectToPickUp + data_kind: inputs + data_key: in + - node_name: EnumExeuctionResult + data_kind: outputs + data_key: CANCELED + - node_name: EnumExeuctionResult + data_kind: outputs + data_key: EXECUTION_FAILURE + - node_name: EnumExeuctionResult + data_kind: outputs + data_key: INVALID_ACTION + - node_name: EnumExeuctionResult + data_kind: outputs + data_key: PLANNING_FAILURE + - node_name: EnumExeuctionResult + data_kind: outputs + data_key: POSTCONDITION_FAILURE + - node_name: EnumExeuctionResult + data_kind: outputs + data_key: PRECONDITION_FAILURE + - node_name: EnumExeuctionResult + data_kind: outputs + data_key: STATUS__DEFAULT + - node_name: EnumExeuctionResult + data_kind: outputs + data_key: UNKNOWN + - node_name: PickUpResult + data_kind: outputs + data_key: message diff --git a/technologies/ros_bt_py/ros_bt_py_pyrobosim/trees/plan_path.yaml b/technologies/ros_bt_py/ros_bt_py_pyrobosim/trees/plan_path.yaml new file mode 100644 index 0000000..78a1f56 --- /dev/null +++ b/technologies/ros_bt_py/ros_bt_py_pyrobosim/trees/plan_path.yaml @@ -0,0 +1,391 @@ +name: '' +path: '' +root_name: MemorySequence +nodes: + - module: ros_bt_py.nodes.sequence + node_class: MemorySequence + version: 0.1.0 + max_children: -1 + name: MemorySequence + child_names: + - SetupSequence + - PlanPathAction + - GetPlanPathResult + - PlanPathSuccess + - FoundPath + options: [] + inputs: [] + outputs: [] + state: SHUTDOWN + - module: ros_bt_py.nodes.sequence + node_class: MemorySequence + version: 0.1.0 + max_children: -1 + name: SetupSequence + child_names: + - TargetLocationIO + - ZeroPose + - Constant + - SetOrientation + - EmptyPath + - EnumExecutionResult + options: [] + inputs: [] + outputs: [] + state: SHUTDOWN + - module: ros_bt_py.nodes.io + node_class: IOInputOption + version: 0.1.0 + max_children: 0 + name: TargetLocationIO + child_names: [] + options: + - key: io_type + serialized_value: '{"py/type": "builtins.str"}' + serialized_type: '{"py/type": "builtins.type"}' + - key: default + serialized_value: '"charger"' + serialized_type: '{"py/type": "builtins.str"}' + inputs: + - key: in + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + outputs: + - key: out + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + state: SHUTDOWN + - module: ros_bt_py.ros_nodes.messages_from_dict + node_class: MessageFromConstDict + version: 0.9.0 + max_children: 0 + name: ZeroPose + child_names: [] + options: + - key: message_type + serialized_value: '{"py/type": "geometry_msgs.msg._pose.Pose"}' + serialized_type: '{"py/type": "builtins.type"}' + - key: dict + serialized_value: '{}' + serialized_type: '{"py/type": "builtins.dict"}' + inputs: [] + outputs: + - key: message + serialized_value: 'null' + serialized_type: '{"py/type": "geometry_msgs.msg._pose.Pose"}' + state: SHUTDOWN + - module: ros_bt_py.nodes.constant + node_class: Constant + version: 0.1.0 + max_children: 0 + name: Constant + child_names: [] + options: + - key: constant_type + serialized_value: '{"py/type": "builtins.float"}' + serialized_type: '{"py/type": "builtins.type"}' + - key: constant_value + serialized_value: '1.0' + serialized_type: '{"py/type": "builtins.float"}' + inputs: [] + outputs: + - key: constant + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.float"}' + state: SHUTDOWN + - module: ros_bt_py.nodes.setters + node_class: SetAttr + version: 0.1.0 + max_children: 0 + name: SetOrientation + child_names: [] + options: + - key: object_type + serialized_value: '{"py/type": "geometry_msgs.msg._pose.Pose"}' + serialized_type: '{"py/type": "builtins.type"}' + - key: attr_name + serialized_value: '"orientation.w"' + serialized_type: '{"py/type": "builtins.str"}' + - key: attr_type + serialized_value: '{"py/type": "builtins.float"}' + serialized_type: '{"py/type": "builtins.type"}' + inputs: + - key: object + serialized_value: 'null' + serialized_type: '{"py/type": "geometry_msgs.msg._pose.Pose"}' + - key: attr_value + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.float"}' + outputs: + - key: new_object + serialized_value: 'null' + serialized_type: '{"py/type": "geometry_msgs.msg._pose.Pose"}' + state: SHUTDOWN + - module: ros_bt_py.ros_nodes.messages_from_dict + node_class: MessageFromConstDict + version: 0.9.0 + max_children: 0 + name: EmptyPath + child_names: [] + options: + - key: message_type + serialized_value: '{"py/type": "pyrobosim_msgs.msg._path.Path"}' + serialized_type: '{"py/type": "builtins.type"}' + - key: dict + serialized_value: '{}' + serialized_type: '{"py/type": "builtins.dict"}' + inputs: [] + outputs: + - key: message + serialized_value: 'null' + serialized_type: '{"py/type": "pyrobosim_msgs.msg._path.Path"}' + state: SHUTDOWN + - module: ros_bt_py.ros_nodes.enum + node_class: EnumFields + version: 0.1.0 + max_children: 0 + name: EnumExecutionResult + child_names: [] + options: + - key: ros_message_type + serialized_value: '{"py/type": "pyrobosim_msgs.msg._execution_result.ExecutionResult"}' + serialized_type: '{"py/type": "builtins.type"}' + inputs: [] + outputs: + - key: CANCELED + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: EXECUTION_FAILURE + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: INVALID_ACTION + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: PLANNING_FAILURE + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: POSTCONDITION_FAILURE + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: PRECONDITION_FAILURE + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: STATUS__DEFAULT + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: SUCCESS + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: UNKNOWN + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + state: SHUTDOWN + - module: ros_bt_py.ros_nodes.action + node_class: Action + version: 0.1.0 + max_children: 0 + name: PlanPathAction + child_names: [] + options: + - key: action_type + serialized_value: '{"py/type": "pyrobosim_msgs.action._plan_path.PlanPath"}' + serialized_type: '{"py/type": "builtins.type"}' + - key: action_name + serialized_value: '"/robot/plan_path"' + serialized_type: '{"py/type": "builtins.str"}' + - key: wait_for_action_server_seconds + serialized_value: '2.0' + serialized_type: '{"py/type": "builtins.float"}' + - key: timeout_seconds + serialized_value: '5.0' + serialized_type: '{"py/type": "builtins.float"}' + - key: fail_if_not_available + serialized_value: 'true' + serialized_type: '{"py/type": "builtins.bool"}' + inputs: + - key: target_location + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + - key: target_pose + serialized_value: 'null' + serialized_type: '{"py/type": "geometry_msgs.msg._pose.Pose"}' + outputs: + - key: result_execution_result + serialized_value: 'null' + serialized_type: '{"py/type": "pyrobosim_msgs.msg._execution_result.ExecutionResult"}' + - key: result_path + serialized_value: 'null' + serialized_type: '{"py/type": "pyrobosim_msgs.msg._path.Path"}' + state: SHUTDOWN + - module: ros_bt_py.ros_nodes.message_converters + node_class: MessageToFields + version: 0.1.0 + max_children: 0 + name: GetPlanPathResult + child_names: [] + options: + - key: input_type + serialized_value: '{"py/type": "pyrobosim_msgs.msg._execution_result.ExecutionResult"}' + serialized_type: '{"py/type": "builtins.type"}' + inputs: + - key: in + serialized_value: 'null' + serialized_type: '{"py/type": "pyrobosim_msgs.msg._execution_result.ExecutionResult"}' + outputs: + - key: status + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: message + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + state: SHUTDOWN + - module: ros_bt_py.nodes.compare + node_class: Compare + version: 0.1.0 + max_children: 0 + name: PlanPathSuccess + child_names: [] + options: + - key: compare_type + serialized_value: '{"py/type": "builtins.int"}' + serialized_type: '{"py/type": "builtins.type"}' + inputs: + - key: a + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: b + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + outputs: [] + state: SHUTDOWN + - module: ros_bt_py.nodes.io + node_class: IOOutput + version: 0.1.0 + max_children: 0 + name: FoundPath + child_names: [] + options: + - key: io_type + serialized_value: '{"py/type": "pyrobosim_msgs.msg._path.Path"}' + serialized_type: '{"py/type": "builtins.type"}' + inputs: + - key: in + serialized_value: 'null' + serialized_type: '{"py/type": "pyrobosim_msgs.msg._path.Path"}' + - key: default + serialized_value: 'null' + serialized_type: '{"py/type": "pyrobosim_msgs.msg._path.Path"}' + outputs: + - key: out + serialized_value: 'null' + serialized_type: '{"py/type": "pyrobosim_msgs.msg._path.Path"}' + state: SHUTDOWN +data_wirings: + - source: + node_name: TargetLocationIO + data_kind: outputs + data_key: out + target: + node_name: PlanPathAction + data_kind: inputs + data_key: target_location + - source: + node_name: PlanPathAction + data_kind: outputs + data_key: result_execution_result + target: + node_name: GetPlanPathResult + data_kind: inputs + data_key: in + - source: + node_name: GetPlanPathResult + data_kind: outputs + data_key: status + target: + node_name: PlanPathSuccess + data_kind: inputs + data_key: a + - source: + node_name: EnumExecutionResult + data_kind: outputs + data_key: SUCCESS + target: + node_name: PlanPathSuccess + data_kind: inputs + data_key: b + - source: + node_name: PlanPathAction + data_kind: outputs + data_key: result_path + target: + node_name: FoundPath + data_kind: inputs + data_key: in + - source: + node_name: EmptyPath + data_kind: outputs + data_key: message + target: + node_name: FoundPath + data_kind: inputs + data_key: default + - source: + node_name: SetOrientation + data_kind: outputs + data_key: new_object + target: + node_name: PlanPathAction + data_kind: inputs + data_key: target_pose + - source: + node_name: ZeroPose + data_kind: outputs + data_key: message + target: + node_name: SetOrientation + data_kind: inputs + data_key: object + - source: + node_name: Constant + data_kind: outputs + data_key: constant + target: + node_name: SetOrientation + data_kind: inputs + data_key: attr_value +tick_frequency_hz: 10 +state: EDITABLE +public_node_data: + - node_name: TargetLocationIO + data_kind: inputs + data_key: in + - node_name: EnumExecutionResult + data_kind: outputs + data_key: CANCELED + - node_name: EnumExecutionResult + data_kind: outputs + data_key: EXECUTION_FAILURE + - node_name: EnumExecutionResult + data_kind: outputs + data_key: INVALID_ACTION + - node_name: EnumExecutionResult + data_kind: outputs + data_key: PLANNING_FAILURE + - node_name: EnumExecutionResult + data_kind: outputs + data_key: POSTCONDITION_FAILURE + - node_name: EnumExecutionResult + data_kind: outputs + data_key: PRECONDITION_FAILURE + - node_name: EnumExecutionResult + data_kind: outputs + data_key: STATUS__DEFAULT + - node_name: EnumExecutionResult + data_kind: outputs + data_key: UNKNOWN + - node_name: GetPlanPathResult + data_kind: outputs + data_key: message + - node_name: FoundPath + data_kind: outputs + data_key: out diff --git a/technologies/ros_bt_py/ros_bt_py_pyrobosim/trees/problem_1.yaml b/technologies/ros_bt_py/ros_bt_py_pyrobosim/trees/problem_1.yaml new file mode 100644 index 0000000..979700a --- /dev/null +++ b/technologies/ros_bt_py/ros_bt_py_pyrobosim/trees/problem_1.yaml @@ -0,0 +1,908 @@ +name: problem_1.yaml +path: '' +root_name: MemorySequence +nodes: + - module: ros_bt_py.nodes.sequence + node_class: MemorySequence + version: 0.1.0 + max_children: -1 + name: MemorySequence + child_names: + - SetupSequence + - RepeatMoveToPantry + - DetectItemsInPantry + options: [] + inputs: [] + outputs: [] + state: SHUTDOWN + - module: ros_bt_py.nodes.sequence + node_class: MemorySequence + version: 0.1.0 + max_children: -1 + name: SetupSequence + child_names: + - PantryName + - DiningRoomTableName + - RequestedObjectCategory + - EnumExecutionResult + options: [] + inputs: [] + outputs: [] + state: SHUTDOWN + - module: ros_bt_py.nodes.constant + node_class: Constant + version: 0.1.0 + max_children: 0 + name: PantryName + child_names: [] + options: + - key: constant_type + serialized_value: '{"py/type": "builtins.str"}' + serialized_type: '{"py/type": "builtins.type"}' + - key: constant_value + serialized_value: '"pantry"' + serialized_type: '{"py/type": "builtins.str"}' + inputs: [] + outputs: + - key: constant + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + state: SHUTDOWN + - module: ros_bt_py.nodes.constant + node_class: Constant + version: 0.1.0 + max_children: 0 + name: DiningRoomTableName + child_names: [] + options: + - key: constant_type + serialized_value: '{"py/type": "builtins.str"}' + serialized_type: '{"py/type": "builtins.type"}' + - key: constant_value + serialized_value: '"table"' + serialized_type: '{"py/type": "builtins.str"}' + inputs: [] + outputs: + - key: constant + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + state: SHUTDOWN + - module: ros_bt_py.nodes.constant + node_class: Constant + version: 0.1.0 + max_children: 0 + name: RequestedObjectCategory + child_names: [] + options: + - key: constant_type + serialized_value: '{"py/type": "builtins.str"}' + serialized_type: '{"py/type": "builtins.type"}' + - key: constant_value + serialized_value: '"snacks"' + serialized_type: '{"py/type": "builtins.str"}' + inputs: [] + outputs: + - key: constant + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + state: SHUTDOWN + - module: ros_bt_py.ros_nodes.enum + node_class: EnumFields + version: 0.1.0 + max_children: 0 + name: EnumExecutionResult + child_names: [] + options: + - key: ros_message_type + serialized_value: '{"py/type": "pyrobosim_msgs.msg._execution_result.ExecutionResult"}' + serialized_type: '{"py/type": "builtins.type"}' + inputs: [] + outputs: + - key: CANCELED + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: EXECUTION_FAILURE + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: INVALID_ACTION + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: PLANNING_FAILURE + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: POSTCONDITION_FAILURE + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: PRECONDITION_FAILURE + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: STATUS__DEFAULT + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: SUCCESS + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: UNKNOWN + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + state: SHUTDOWN + - module: ros_bt_py.nodes.decorators + node_class: RepeatIfFail + version: 0.1.0 + max_children: 1 + name: RepeatMoveToPantry + child_names: + - MoveToPantry + options: [] + inputs: [] + outputs: [] + state: SHUTDOWN + - module: ros_bt_py.ros_nodes.subtree + node_class: Subtree + version: 0.2.0 + max_children: 0 + name: MoveToPantry + child_names: [] + options: + - key: subtree_path + serialized_value: '"package://ros_bt_py_pyrobosim/trees/move_to_pose.yaml"' + serialized_type: '{"py/type": "builtins.str"}' + - key: use_io_nodes + serialized_value: 'true' + serialized_type: '{"py/type": "builtins.bool"}' + inputs: + - key: TargetPoseInput.in + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + outputs: + - key: load_success + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.bool"}' + - key: load_error_msg + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + state: SHUTDOWN + - module: ros_bt_py.nodes.sequence + node_class: MemorySequence + version: 0.1.0 + max_children: -1 + name: DetectItemsInPantry + child_names: + - DetectObjectAction + - GetDetectObjectResults + - ObjectDetectionSuccessfull? + - ListLength + - Inverter + - IterateList + options: [] + inputs: [] + outputs: [] + state: SHUTDOWN + - module: ros_bt_py.ros_nodes.action + node_class: Action + version: 0.1.0 + max_children: 0 + name: DetectObjectAction + child_names: [] + options: + - key: action_type + serialized_value: '{"py/type": "pyrobosim_msgs.action._detect_objects.DetectObjects"}' + serialized_type: '{"py/type": "builtins.type"}' + - key: action_name + serialized_value: '"/robot/detect_objects"' + serialized_type: '{"py/type": "builtins.str"}' + - key: wait_for_action_server_seconds + serialized_value: '1.2' + serialized_type: '{"py/type": "builtins.float"}' + - key: timeout_seconds + serialized_value: '5.0' + serialized_type: '{"py/type": "builtins.float"}' + - key: fail_if_not_available + serialized_value: 'true' + serialized_type: '{"py/type": "builtins.bool"}' + inputs: + - key: target_object + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + outputs: + - key: result_execution_result + serialized_value: 'null' + serialized_type: '{"py/type": "pyrobosim_msgs.msg._execution_result.ExecutionResult"}' + - key: result_detected_objects + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.list"}' + state: SHUTDOWN + - module: ros_bt_py.ros_nodes.message_converters + node_class: MessageToFields + version: 0.1.0 + max_children: 0 + name: GetDetectObjectResults + child_names: [] + options: + - key: input_type + serialized_value: '{"py/type": "pyrobosim_msgs.msg._execution_result.ExecutionResult"}' + serialized_type: '{"py/type": "builtins.type"}' + inputs: + - key: in + serialized_value: 'null' + serialized_type: '{"py/type": "pyrobosim_msgs.msg._execution_result.ExecutionResult"}' + outputs: + - key: status + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: message + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + state: SHUTDOWN + - module: ros_bt_py.nodes.compare + node_class: Compare + version: 0.1.0 + max_children: 0 + name: ObjectDetectionSuccessfull? + child_names: [] + options: + - key: compare_type + serialized_value: '{"py/type": "builtins.int"}' + serialized_type: '{"py/type": "builtins.type"}' + inputs: + - key: a + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: b + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + outputs: [] + state: SHUTDOWN + - module: ros_bt_py.nodes.list + node_class: ListLength + version: 0.1.0 + max_children: 0 + name: ListLength + child_names: [] + options: [] + inputs: + - key: list + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.list"}' + outputs: + - key: length + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + state: SHUTDOWN + - module: ros_bt_py.nodes.decorators + node_class: Inverter + version: 0.1.0 + max_children: 1 + name: Inverter + child_names: + - LessThanIntConstant + options: [] + inputs: [] + outputs: [] + state: SHUTDOWN + - module: ros_bt_py.nodes.compare + node_class: LessThanIntConstant + version: 0.1.0 + max_children: 0 + name: LessThanIntConstant + child_names: [] + options: + - key: target + serialized_value: '1' + serialized_type: '{"py/type": "builtins.int"}' + inputs: + - key: a + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + outputs: [] + state: SHUTDOWN + - module: ros_bt_py.nodes.list + node_class: IterateList + version: 0.1.0 + max_children: 1 + name: IterateList + child_names: + - MoveObject + options: + - key: item_type + serialized_value: '{"py/type": "pyrobosim_msgs.msg._object_state.ObjectState"}' + serialized_type: '{"py/type": "builtins.type"}' + inputs: + - key: list + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.list"}' + outputs: + - key: list_item + serialized_value: 'null' + serialized_type: '{"py/type": "pyrobosim_msgs.msg._object_state.ObjectState"}' + state: SHUTDOWN + - module: ros_bt_py.nodes.sequence + node_class: MemorySequence + version: 0.1.0 + max_children: -1 + name: MoveObject + child_names: + - GetFoundObjectAttributes + - PickUpSequence + - RepeatMoveToDiningRoomTable + - PlaceDownSequence + - MoveToPantrySequence + options: [] + inputs: [] + outputs: [] + state: SHUTDOWN + - module: ros_bt_py.ros_nodes.message_converters + node_class: MessageToFields + version: 0.1.0 + max_children: 0 + name: GetFoundObjectAttributes + child_names: [] + options: + - key: input_type + serialized_value: '{"py/type": "pyrobosim_msgs.msg._object_state.ObjectState"}' + serialized_type: '{"py/type": "builtins.type"}' + inputs: + - key: in + serialized_value: 'null' + serialized_type: '{"py/type": "pyrobosim_msgs.msg._object_state.ObjectState"}' + outputs: + - key: name + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + - key: category + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + - key: parent + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + - key: pose + serialized_value: 'null' + serialized_type: '{"py/type": "geometry_msgs.msg._pose.Pose"}' + state: SHUTDOWN + - module: ros_bt_py.nodes.sequence + node_class: MemorySequence + version: 0.1.0 + max_children: -1 + name: PickUpSequence + child_names: + - PickUpRequest + - SetAttr + - PickUpObject + - MessageToFields_2 + - Compare + options: [] + inputs: [] + outputs: [] + state: SHUTDOWN + - module: ros_bt_py.ros_nodes.messages_from_dict + node_class: MessageFromConstDict + version: 0.9.0 + max_children: 0 + name: PickUpRequest + child_names: [] + options: + - key: message_type + serialized_value: '{"py/type": "pyrobosim_msgs.msg._task_action.TaskAction"}' + serialized_type: '{"py/type": "builtins.type"}' + - key: dict + serialized_value: '{"type": "pick", "robot": "robot"}' + serialized_type: '{"py/type": "builtins.dict"}' + inputs: [] + outputs: + - key: message + serialized_value: 'null' + serialized_type: '{"py/type": "pyrobosim_msgs.msg._task_action.TaskAction"}' + state: SHUTDOWN + - module: ros_bt_py.nodes.setters + node_class: SetAttr + version: 0.1.0 + max_children: 0 + name: SetAttr + child_names: [] + options: + - key: object_type + serialized_value: '{"py/type": "pyrobosim_msgs.msg._task_action.TaskAction"}' + serialized_type: '{"py/type": "builtins.type"}' + - key: attr_name + serialized_value: '"object"' + serialized_type: '{"py/type": "builtins.str"}' + - key: attr_type + serialized_value: '{"py/type": "builtins.str"}' + serialized_type: '{"py/type": "builtins.type"}' + inputs: + - key: object + serialized_value: 'null' + serialized_type: '{"py/type": "pyrobosim_msgs.msg._task_action.TaskAction"}' + - key: attr_value + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + outputs: + - key: new_object + serialized_value: 'null' + serialized_type: '{"py/type": "pyrobosim_msgs.msg._task_action.TaskAction"}' + state: SHUTDOWN + - module: ros_bt_py.ros_nodes.action + node_class: Action + version: 0.1.0 + max_children: 0 + name: PickUpObject + child_names: [] + options: + - key: action_type + serialized_value: >- + {"py/type": + "pyrobosim_msgs.action._execute_task_action.ExecuteTaskAction"} + serialized_type: '{"py/type": "builtins.type"}' + - key: action_name + serialized_value: '"/execute_action"' + serialized_type: '{"py/type": "builtins.str"}' + - key: wait_for_action_server_seconds + serialized_value: '1.2' + serialized_type: '{"py/type": "builtins.float"}' + - key: timeout_seconds + serialized_value: '5.0' + serialized_type: '{"py/type": "builtins.float"}' + - key: fail_if_not_available + serialized_value: 'true' + serialized_type: '{"py/type": "builtins.bool"}' + inputs: + - key: action + serialized_value: 'null' + serialized_type: '{"py/type": "pyrobosim_msgs.msg._task_action.TaskAction"}' + outputs: + - key: result_execution_result + serialized_value: 'null' + serialized_type: '{"py/type": "pyrobosim_msgs.msg._execution_result.ExecutionResult"}' + state: SHUTDOWN + - module: ros_bt_py.ros_nodes.message_converters + node_class: MessageToFields + version: 0.1.0 + max_children: 0 + name: MessageToFields_2 + child_names: [] + options: + - key: input_type + serialized_value: '{"py/type": "pyrobosim_msgs.msg._execution_result.ExecutionResult"}' + serialized_type: '{"py/type": "builtins.type"}' + inputs: + - key: in + serialized_value: 'null' + serialized_type: '{"py/type": "pyrobosim_msgs.msg._execution_result.ExecutionResult"}' + outputs: + - key: status + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: message + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + state: SHUTDOWN + - module: ros_bt_py.nodes.compare + node_class: Compare + version: 0.1.0 + max_children: 0 + name: Compare + child_names: [] + options: + - key: compare_type + serialized_value: '{"py/type": "builtins.int"}' + serialized_type: '{"py/type": "builtins.type"}' + inputs: + - key: a + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: b + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + outputs: [] + state: SHUTDOWN + - module: ros_bt_py.nodes.decorators + node_class: RepeatIfFail + version: 0.1.0 + max_children: 1 + name: RepeatMoveToDiningRoomTable + child_names: + - MoveToDiningTable + options: [] + inputs: [] + outputs: [] + state: SHUTDOWN + - module: ros_bt_py.ros_nodes.subtree + node_class: Subtree + version: 0.2.0 + max_children: 0 + name: MoveToDiningTable + child_names: [] + options: + - key: subtree_path + serialized_value: '"package://ros_bt_py_pyrobosim/trees/move_to_pose.yaml"' + serialized_type: '{"py/type": "builtins.str"}' + - key: use_io_nodes + serialized_value: 'true' + serialized_type: '{"py/type": "builtins.bool"}' + inputs: + - key: TargetPoseInput.in + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + outputs: + - key: load_success + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.bool"}' + - key: load_error_msg + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + state: SHUTDOWN + - module: ros_bt_py.nodes.sequence + node_class: MemorySequence + version: 0.1.0 + max_children: -1 + name: PlaceDownSequence + child_names: + - PlaceDownTask + - PlaceDownObject + - GetPlaceDownResult + - PlaceDownSuccessful? + options: [] + inputs: [] + outputs: [] + state: SHUTDOWN + - module: ros_bt_py.ros_nodes.messages_from_dict + node_class: MessageFromConstDict + version: 0.9.0 + max_children: 0 + name: PlaceDownTask + child_names: [] + options: + - key: message_type + serialized_value: '{"py/type": "pyrobosim_msgs.msg._task_action.TaskAction"}' + serialized_type: '{"py/type": "builtins.type"}' + - key: dict + serialized_value: '{"type": "place", "robot": "robot"}' + serialized_type: '{"py/type": "builtins.dict"}' + inputs: [] + outputs: + - key: message + serialized_value: 'null' + serialized_type: '{"py/type": "pyrobosim_msgs.msg._task_action.TaskAction"}' + state: SHUTDOWN + - module: ros_bt_py.ros_nodes.action + node_class: Action + version: 0.1.0 + max_children: 0 + name: PlaceDownObject + child_names: [] + options: + - key: action_type + serialized_value: >- + {"py/type": + "pyrobosim_msgs.action._execute_task_action.ExecuteTaskAction"} + serialized_type: '{"py/type": "builtins.type"}' + - key: action_name + serialized_value: '"/execute_action"' + serialized_type: '{"py/type": "builtins.str"}' + - key: wait_for_action_server_seconds + serialized_value: '1.2' + serialized_type: '{"py/type": "builtins.float"}' + - key: timeout_seconds + serialized_value: '5.0' + serialized_type: '{"py/type": "builtins.float"}' + - key: fail_if_not_available + serialized_value: 'true' + serialized_type: '{"py/type": "builtins.bool"}' + inputs: + - key: action + serialized_value: 'null' + serialized_type: '{"py/type": "pyrobosim_msgs.msg._task_action.TaskAction"}' + outputs: + - key: result_execution_result + serialized_value: 'null' + serialized_type: '{"py/type": "pyrobosim_msgs.msg._execution_result.ExecutionResult"}' + state: SHUTDOWN + - module: ros_bt_py.ros_nodes.message_converters + node_class: MessageToFields + version: 0.1.0 + max_children: 0 + name: GetPlaceDownResult + child_names: [] + options: + - key: input_type + serialized_value: '{"py/type": "pyrobosim_msgs.msg._execution_result.ExecutionResult"}' + serialized_type: '{"py/type": "builtins.type"}' + inputs: + - key: in + serialized_value: 'null' + serialized_type: '{"py/type": "pyrobosim_msgs.msg._execution_result.ExecutionResult"}' + outputs: + - key: status + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: message + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + state: SHUTDOWN + - module: ros_bt_py.nodes.compare + node_class: Compare + version: 0.1.0 + max_children: 0 + name: PlaceDownSuccessful? + child_names: [] + options: + - key: compare_type + serialized_value: '{"py/type": "builtins.int"}' + serialized_type: '{"py/type": "builtins.type"}' + inputs: + - key: a + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: b + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + outputs: [] + state: SHUTDOWN + - module: ros_bt_py.nodes.sequence + node_class: MemorySequence + version: 0.1.0 + max_children: -1 + name: MoveToPantrySequence + child_names: + - MoveToPantry_2 + options: [] + inputs: [] + outputs: [] + state: SHUTDOWN + - module: ros_bt_py.ros_nodes.subtree + node_class: Subtree + version: 0.2.0 + max_children: 0 + name: MoveToPantry_2 + child_names: [] + options: + - key: subtree_path + serialized_value: '"package://ros_bt_py_pyrobosim/trees/move_to_pose.yaml"' + serialized_type: '{"py/type": "builtins.str"}' + - key: use_io_nodes + serialized_value: 'true' + serialized_type: '{"py/type": "builtins.bool"}' + inputs: + - key: TargetPoseInput.in + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + outputs: + - key: load_success + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.bool"}' + - key: load_error_msg + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + state: SHUTDOWN +data_wirings: + - source: + node_name: PantryName + data_kind: outputs + data_key: constant + target: + node_name: MoveToPantry + data_kind: inputs + data_key: TargetPoseInput.in + - source: + node_name: DiningRoomTableName + data_kind: outputs + data_key: constant + target: + node_name: MoveToDiningTable + data_kind: inputs + data_key: TargetPoseInput.in + - source: + node_name: GetDetectObjectResults + data_kind: outputs + data_key: status + target: + node_name: ObjectDetectionSuccessfull? + data_kind: inputs + data_key: a + - source: + node_name: EnumExecutionResult + data_kind: outputs + data_key: SUCCESS + target: + node_name: ObjectDetectionSuccessfull? + data_kind: inputs + data_key: b + - source: + node_name: ListLength + data_kind: outputs + data_key: length + target: + node_name: LessThanIntConstant + data_kind: inputs + data_key: a + - source: + node_name: DetectObjectAction + data_kind: outputs + data_key: result_execution_result + target: + node_name: GetDetectObjectResults + data_kind: inputs + data_key: in + - source: + node_name: DetectObjectAction + data_kind: outputs + data_key: result_detected_objects + target: + node_name: ListLength + data_kind: inputs + data_key: list + - source: + node_name: DetectObjectAction + data_kind: outputs + data_key: result_detected_objects + target: + node_name: IterateList + data_kind: inputs + data_key: list + - source: + node_name: SetAttr + data_kind: outputs + data_key: new_object + target: + node_name: PickUpObject + data_kind: inputs + data_key: action + - source: + node_name: PickUpObject + data_kind: outputs + data_key: result_execution_result + target: + node_name: MessageToFields_2 + data_kind: inputs + data_key: in + - source: + node_name: MessageToFields_2 + data_kind: outputs + data_key: status + target: + node_name: Compare + data_kind: inputs + data_key: b + - source: + node_name: EnumExecutionResult + data_kind: outputs + data_key: SUCCESS + target: + node_name: Compare + data_kind: inputs + data_key: a + - source: + node_name: PlaceDownTask + data_kind: outputs + data_key: message + target: + node_name: PlaceDownObject + data_kind: inputs + data_key: action + - source: + node_name: PlaceDownObject + data_kind: outputs + data_key: result_execution_result + target: + node_name: GetPlaceDownResult + data_kind: inputs + data_key: in + - source: + node_name: EnumExecutionResult + data_kind: outputs + data_key: SUCCESS + target: + node_name: PlaceDownSuccessful? + data_kind: inputs + data_key: a + - source: + node_name: GetPlaceDownResult + data_kind: outputs + data_key: status + target: + node_name: PlaceDownSuccessful? + data_kind: inputs + data_key: b + - source: + node_name: PickUpRequest + data_kind: outputs + data_key: message + target: + node_name: SetAttr + data_kind: inputs + data_key: object + - source: + node_name: IterateList + data_kind: outputs + data_key: list_item + target: + node_name: GetFoundObjectAttributes + data_kind: inputs + data_key: in + - source: + node_name: GetFoundObjectAttributes + data_kind: outputs + data_key: name + target: + node_name: SetAttr + data_kind: inputs + data_key: attr_value + - source: + node_name: RequestedObjectCategory + data_kind: outputs + data_key: constant + target: + node_name: DetectObjectAction + data_kind: inputs + data_key: target_object + - source: + node_name: PantryName + data_kind: outputs + data_key: constant + target: + node_name: MoveToPantry_2 + data_kind: inputs + data_key: TargetPoseInput.in +tick_frequency_hz: 10 +state: EDITABLE +public_node_data: + - node_name: EnumExecutionResult + data_kind: outputs + data_key: CANCELED + - node_name: EnumExecutionResult + data_kind: outputs + data_key: EXECUTION_FAILURE + - node_name: EnumExecutionResult + data_kind: outputs + data_key: INVALID_ACTION + - node_name: EnumExecutionResult + data_kind: outputs + data_key: PLANNING_FAILURE + - node_name: EnumExecutionResult + data_kind: outputs + data_key: POSTCONDITION_FAILURE + - node_name: EnumExecutionResult + data_kind: outputs + data_key: PRECONDITION_FAILURE + - node_name: EnumExecutionResult + data_kind: outputs + data_key: STATUS__DEFAULT + - node_name: EnumExecutionResult + data_kind: outputs + data_key: UNKNOWN + - node_name: MoveToPantry + data_kind: outputs + data_key: load_success + - node_name: MoveToPantry + data_kind: outputs + data_key: load_error_msg + - node_name: GetDetectObjectResults + data_kind: outputs + data_key: message + - node_name: GetFoundObjectAttributes + data_kind: outputs + data_key: category + - node_name: GetFoundObjectAttributes + data_kind: outputs + data_key: parent + - node_name: GetFoundObjectAttributes + data_kind: outputs + data_key: pose + - node_name: MessageToFields_2 + data_kind: outputs + data_key: message + - node_name: MoveToDiningTable + data_kind: outputs + data_key: load_success + - node_name: MoveToDiningTable + data_kind: outputs + data_key: load_error_msg + - node_name: GetPlaceDownResult + data_kind: outputs + data_key: message + - node_name: MoveToPantry_2 + data_kind: outputs + data_key: load_success + - node_name: MoveToPantry_2 + data_kind: outputs + data_key: load_error_msg diff --git a/technologies/ros_bt_py/ros_bt_py_pyrobosim/trees/problem_2.yaml b/technologies/ros_bt_py/ros_bt_py_pyrobosim/trees/problem_2.yaml new file mode 100644 index 0000000..4428b54 --- /dev/null +++ b/technologies/ros_bt_py/ros_bt_py_pyrobosim/trees/problem_2.yaml @@ -0,0 +1,957 @@ +name: problem_2.yaml +path: '' +root_name: MemorySequence +nodes: + - module: ros_bt_py.nodes.sequence + node_class: MemorySequence + version: 0.1.0 + max_children: -1 + name: MemorySequence + child_names: + - SetupSequence + - PrepareRoute + - IterateList + options: [] + inputs: [] + outputs: [] + state: SHUTDOWN + - module: ros_bt_py.nodes.sequence + node_class: MemorySequence + version: 0.1.0 + max_children: -1 + name: SetupSequence + child_names: + - EnumExeuctionResult + - Dumpster + - WasteComponentName + - DiningTrashHallway + - Table + - TrashLocations + options: [] + inputs: [] + outputs: [] + state: SHUTDOWN + - module: ros_bt_py.ros_nodes.enum + node_class: EnumFields + version: 0.1.0 + max_children: 0 + name: EnumExeuctionResult + child_names: [] + options: + - key: ros_message_type + serialized_value: '{"py/type": "pyrobosim_msgs.msg._execution_result.ExecutionResult"}' + serialized_type: '{"py/type": "builtins.type"}' + inputs: [] + outputs: + - key: CANCELED + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: EXECUTION_FAILURE + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: INVALID_ACTION + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: PLANNING_FAILURE + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: POSTCONDITION_FAILURE + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: PRECONDITION_FAILURE + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: STATUS__DEFAULT + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: SUCCESS + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: UNKNOWN + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + state: SHUTDOWN + - module: ros_bt_py.nodes.constant + node_class: Constant + version: 0.1.0 + max_children: 0 + name: Dumpster + child_names: [] + options: + - key: constant_type + serialized_value: '{"py/type": "builtins.str"}' + serialized_type: '{"py/type": "builtins.type"}' + - key: constant_value + serialized_value: '"dumpster"' + serialized_type: '{"py/type": "builtins.str"}' + inputs: [] + outputs: + - key: constant + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + state: SHUTDOWN + - module: ros_bt_py.nodes.constant + node_class: Constant + version: 0.1.0 + max_children: 0 + name: WasteComponentName + child_names: [] + options: + - key: constant_type + serialized_value: '{"py/type": "builtins.str"}' + serialized_type: '{"py/type": "builtins.type"}' + - key: constant_value + serialized_value: '"waste"' + serialized_type: '{"py/type": "builtins.str"}' + inputs: [] + outputs: + - key: constant + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + state: SHUTDOWN + - module: ros_bt_py.nodes.constant + node_class: Constant + version: 0.1.0 + max_children: 0 + name: DiningTrashHallway + child_names: [] + options: + - key: constant_type + serialized_value: '{"py/type": "builtins.str"}' + serialized_type: '{"py/type": "builtins.type"}' + - key: constant_value + serialized_value: '"hall_dining_trash"' + serialized_type: '{"py/type": "builtins.str"}' + inputs: [] + outputs: + - key: constant + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + state: SHUTDOWN + - module: ros_bt_py.nodes.constant + node_class: Constant + version: 0.1.0 + max_children: 0 + name: Table + child_names: [] + options: + - key: constant_type + serialized_value: '{"py/type": "builtins.str"}' + serialized_type: '{"py/type": "builtins.type"}' + - key: constant_value + serialized_value: '"table"' + serialized_type: '{"py/type": "builtins.str"}' + inputs: [] + outputs: + - key: constant + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + state: SHUTDOWN + - module: ros_bt_py.nodes.constant + node_class: Constant + version: 0.1.0 + max_children: 0 + name: TrashLocations + child_names: [] + options: + - key: constant_type + serialized_value: '{"py/type": "builtins.list"}' + serialized_type: '{"py/type": "builtins.type"}' + - key: constant_value + serialized_value: '["bin", "desk"]' + serialized_type: '{"py/type": "builtins.list"}' + inputs: [] + outputs: + - key: constant + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.list"}' + state: SHUTDOWN + - module: ros_bt_py.nodes.sequence + node_class: MemorySequence + version: 0.1.0 + max_children: -1 + name: PrepareRoute + child_names: + - MoveToDump + - OpenDumpster + options: [] + inputs: [] + outputs: [] + state: SHUTDOWN + - module: ros_bt_py.nodes.sequence + node_class: MemorySequence + version: 0.1.0 + max_children: -1 + name: MoveToDump + child_names: + - PlanToDump + - MoveToDumpAction + - GetMoveToDumpActionResult + - MoveToDumpSuccessful? + options: [] + inputs: [] + outputs: [] + state: SHUTDOWN + - module: ros_bt_py.nodes.fallback + node_class: MemoryFallback + version: 0.1.0 + max_children: -1 + name: PlanToDump + child_names: + - PlanPathToDump + - MakeWayToTheTrashRoomSequence + options: [] + inputs: [] + outputs: [] + state: SHUTDOWN + - module: ros_bt_py.ros_nodes.subtree + node_class: Subtree + version: 0.2.0 + max_children: 0 + name: PlanPathToDump + child_names: [] + options: + - key: subtree_path + serialized_value: '"package://ros_bt_py_pyrobosim/trees/plan_path.yaml"' + serialized_type: '{"py/type": "builtins.str"}' + - key: use_io_nodes + serialized_value: 'true' + serialized_type: '{"py/type": "builtins.bool"}' + inputs: + - key: TargetLocationIO.in + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + outputs: + - key: load_success + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.bool"}' + - key: load_error_msg + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + - key: FoundPath.out + serialized_value: 'null' + serialized_type: '{"py/type": "pyrobosim_msgs.msg._path.Path"}' + state: SHUTDOWN + - module: ros_bt_py.nodes.sequence + node_class: MemorySequence + version: 0.1.0 + max_children: -1 + name: MakeWayToTheTrashRoomSequence + child_names: + - MoveToDiningTrashHallway + - OpenDoor + - PlanPathToDump_2 + options: [] + inputs: [] + outputs: [] + state: SHUTDOWN + - module: ros_bt_py.ros_nodes.subtree + node_class: Subtree + version: 0.2.0 + max_children: 0 + name: MoveToDiningTrashHallway + child_names: [] + options: + - key: subtree_path + serialized_value: '"package://ros_bt_py_pyrobosim/trees/move_to_pose.yaml"' + serialized_type: '{"py/type": "builtins.str"}' + - key: use_io_nodes + serialized_value: 'true' + serialized_type: '{"py/type": "builtins.bool"}' + inputs: + - key: TargetPoseInput.in + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + outputs: + - key: load_success + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.bool"}' + - key: load_error_msg + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + state: SHUTDOWN + - module: ros_bt_py.ros_nodes.subtree + node_class: Subtree + version: 0.2.0 + max_children: 0 + name: OpenDoor + child_names: [] + options: + - key: subtree_path + serialized_value: '"package://ros_bt_py_pyrobosim/trees/open_door.yaml"' + serialized_type: '{"py/type": "builtins.str"}' + - key: use_io_nodes + serialized_value: 'true' + serialized_type: '{"py/type": "builtins.bool"}' + inputs: [] + outputs: + - key: load_success + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.bool"}' + - key: load_error_msg + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + state: SHUTDOWN + - module: ros_bt_py.ros_nodes.subtree + node_class: Subtree + version: 0.2.0 + max_children: 0 + name: PlanPathToDump_2 + child_names: [] + options: + - key: subtree_path + serialized_value: '"package://ros_bt_py_pyrobosim/trees/plan_path.yaml"' + serialized_type: '{"py/type": "builtins.str"}' + - key: use_io_nodes + serialized_value: 'true' + serialized_type: '{"py/type": "builtins.bool"}' + inputs: + - key: TargetLocationIO.in + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + outputs: + - key: load_success + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.bool"}' + - key: load_error_msg + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + - key: FoundPath.out + serialized_value: 'null' + serialized_type: '{"py/type": "pyrobosim_msgs.msg._path.Path"}' + state: SHUTDOWN + - module: ros_bt_py.ros_nodes.action + node_class: Action + version: 0.1.0 + max_children: 0 + name: MoveToDumpAction + child_names: [] + options: + - key: action_type + serialized_value: '{"py/type": "pyrobosim_msgs.action._follow_path.FollowPath"}' + serialized_type: '{"py/type": "builtins.type"}' + - key: action_name + serialized_value: '"/robot/follow_path"' + serialized_type: '{"py/type": "builtins.str"}' + - key: wait_for_action_server_seconds + serialized_value: '2.0' + serialized_type: '{"py/type": "builtins.float"}' + - key: timeout_seconds + serialized_value: '30.0' + serialized_type: '{"py/type": "builtins.float"}' + - key: fail_if_not_available + serialized_value: 'true' + serialized_type: '{"py/type": "builtins.bool"}' + inputs: + - key: path + serialized_value: 'null' + serialized_type: '{"py/type": "pyrobosim_msgs.msg._path.Path"}' + outputs: + - key: result_execution_result + serialized_value: 'null' + serialized_type: '{"py/type": "pyrobosim_msgs.msg._execution_result.ExecutionResult"}' + state: SHUTDOWN + - module: ros_bt_py.ros_nodes.message_converters + node_class: MessageToFields + version: 0.1.0 + max_children: 0 + name: GetMoveToDumpActionResult + child_names: [] + options: + - key: input_type + serialized_value: '{"py/type": "pyrobosim_msgs.msg._execution_result.ExecutionResult"}' + serialized_type: '{"py/type": "builtins.type"}' + inputs: + - key: in + serialized_value: 'null' + serialized_type: '{"py/type": "pyrobosim_msgs.msg._execution_result.ExecutionResult"}' + outputs: + - key: status + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: message + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + state: SHUTDOWN + - module: ros_bt_py.nodes.compare + node_class: Compare + version: 0.1.0 + max_children: 0 + name: MoveToDumpSuccessful? + child_names: [] + options: + - key: compare_type + serialized_value: '{"py/type": "builtins.int"}' + serialized_type: '{"py/type": "builtins.type"}' + inputs: + - key: a + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: b + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + outputs: [] + state: SHUTDOWN + - module: ros_bt_py.ros_nodes.subtree + node_class: Subtree + version: 0.2.0 + max_children: 0 + name: OpenDumpster + child_names: [] + options: + - key: subtree_path + serialized_value: '"package://ros_bt_py_pyrobosim/trees/open_door.yaml"' + serialized_type: '{"py/type": "builtins.str"}' + - key: use_io_nodes + serialized_value: 'true' + serialized_type: '{"py/type": "builtins.bool"}' + inputs: [] + outputs: + - key: load_success + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.bool"}' + - key: load_error_msg + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + state: SHUTDOWN + - module: ros_bt_py.nodes.list + node_class: IterateList + version: 0.1.0 + max_children: 1 + name: IterateList + child_names: + - MemorySequence_2 + options: + - key: item_type + serialized_value: '{"py/type": "builtins.str"}' + serialized_type: '{"py/type": "builtins.type"}' + inputs: + - key: list + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.list"}' + outputs: + - key: list_item + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + state: SHUTDOWN + - module: ros_bt_py.nodes.sequence + node_class: MemorySequence + version: 0.1.0 + max_children: -1 + name: MemorySequence_2 + child_names: + - GetTrashFromLocation + - MoveToDumpster_2 + - PutDownTrash + options: [] + inputs: [] + outputs: [] + state: SHUTDOWN + - module: ros_bt_py.nodes.sequence + node_class: MemorySequence + version: 0.1.0 + max_children: -1 + name: GetTrashFromLocation + child_names: + - MoveToTrashLocation + - DetectObjectsSequence + - GetConstListItem + - MessageToFields + - PickUpTrash + options: [] + inputs: [] + outputs: [] + state: SHUTDOWN + - module: ros_bt_py.ros_nodes.subtree + node_class: Subtree + version: 0.2.0 + max_children: 0 + name: MoveToTrashLocation + child_names: [] + options: + - key: subtree_path + serialized_value: '"package://ros_bt_py_pyrobosim/trees/move_to_pose.yaml"' + serialized_type: '{"py/type": "builtins.str"}' + - key: use_io_nodes + serialized_value: 'true' + serialized_type: '{"py/type": "builtins.bool"}' + inputs: + - key: TargetPoseInput.in + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + outputs: + - key: load_success + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.bool"}' + - key: load_error_msg + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + state: SHUTDOWN + - module: ros_bt_py.nodes.sequence + node_class: MemorySequence + version: 0.1.0 + max_children: -1 + name: DetectObjectsSequence + child_names: + - DetectObjects + - GetDetectObjectsResult + - DetectObjectResultSuccessfull? + options: [] + inputs: [] + outputs: [] + state: SHUTDOWN + - module: ros_bt_py.ros_nodes.action + node_class: Action + version: 0.1.0 + max_children: 0 + name: DetectObjects + child_names: [] + options: + - key: action_type + serialized_value: '{"py/type": "pyrobosim_msgs.action._detect_objects.DetectObjects"}' + serialized_type: '{"py/type": "builtins.type"}' + - key: action_name + serialized_value: '"/robot/detect_objects"' + serialized_type: '{"py/type": "builtins.str"}' + - key: wait_for_action_server_seconds + serialized_value: '1.2' + serialized_type: '{"py/type": "builtins.float"}' + - key: timeout_seconds + serialized_value: '5.0' + serialized_type: '{"py/type": "builtins.float"}' + - key: fail_if_not_available + serialized_value: 'true' + serialized_type: '{"py/type": "builtins.bool"}' + inputs: + - key: target_object + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + outputs: + - key: result_execution_result + serialized_value: 'null' + serialized_type: '{"py/type": "pyrobosim_msgs.msg._execution_result.ExecutionResult"}' + - key: result_detected_objects + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.list"}' + state: SHUTDOWN + - module: ros_bt_py.ros_nodes.message_converters + node_class: MessageToFields + version: 0.1.0 + max_children: 0 + name: GetDetectObjectsResult + child_names: [] + options: + - key: input_type + serialized_value: '{"py/type": "pyrobosim_msgs.msg._execution_result.ExecutionResult"}' + serialized_type: '{"py/type": "builtins.type"}' + inputs: + - key: in + serialized_value: 'null' + serialized_type: '{"py/type": "pyrobosim_msgs.msg._execution_result.ExecutionResult"}' + outputs: + - key: status + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: message + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + state: SHUTDOWN + - module: ros_bt_py.nodes.compare + node_class: Compare + version: 0.1.0 + max_children: 0 + name: DetectObjectResultSuccessfull? + child_names: [] + options: + - key: compare_type + serialized_value: '{"py/type": "builtins.int"}' + serialized_type: '{"py/type": "builtins.type"}' + inputs: + - key: a + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: b + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + outputs: [] + state: SHUTDOWN + - module: ros_bt_py.nodes.getters + node_class: GetConstListItem + version: 0.1.0 + max_children: 1 + name: GetConstListItem + child_names: [] + options: + - key: list_type + serialized_value: '{"py/type": "pyrobosim_msgs.msg._object_state.ObjectState"}' + serialized_type: '{"py/type": "builtins.type"}' + - key: index + serialized_value: '0' + serialized_type: '{"py/type": "builtins.int"}' + - key: succeed_on_stale_data + serialized_value: 'true' + serialized_type: '{"py/type": "builtins.bool"}' + inputs: + - key: list + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.list"}' + outputs: + - key: item + serialized_value: 'null' + serialized_type: '{"py/type": "pyrobosim_msgs.msg._object_state.ObjectState"}' + state: SHUTDOWN + - module: ros_bt_py.ros_nodes.message_converters + node_class: MessageToFields + version: 0.1.0 + max_children: 0 + name: MessageToFields + child_names: [] + options: + - key: input_type + serialized_value: '{"py/type": "pyrobosim_msgs.msg._object_state.ObjectState"}' + serialized_type: '{"py/type": "builtins.type"}' + inputs: + - key: in + serialized_value: 'null' + serialized_type: '{"py/type": "pyrobosim_msgs.msg._object_state.ObjectState"}' + outputs: + - key: name + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + - key: category + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + - key: parent + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + - key: pose + serialized_value: 'null' + serialized_type: '{"py/type": "geometry_msgs.msg._pose.Pose"}' + state: SHUTDOWN + - module: ros_bt_py.ros_nodes.subtree + node_class: Subtree + version: 0.2.0 + max_children: 0 + name: PickUpTrash + child_names: [] + options: + - key: subtree_path + serialized_value: '"package://ros_bt_py_pyrobosim/trees/pick_up.yaml"' + serialized_type: '{"py/type": "builtins.str"}' + - key: use_io_nodes + serialized_value: 'true' + serialized_type: '{"py/type": "builtins.bool"}' + inputs: + - key: ObjectToPickUp.in + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + outputs: + - key: load_success + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.bool"}' + - key: load_error_msg + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + state: SHUTDOWN + - module: ros_bt_py.ros_nodes.subtree + node_class: Subtree + version: 0.2.0 + max_children: 0 + name: MoveToDumpster_2 + child_names: [] + options: + - key: subtree_path + serialized_value: '"package://ros_bt_py_pyrobosim/trees/move_to_pose.yaml"' + serialized_type: '{"py/type": "builtins.str"}' + - key: use_io_nodes + serialized_value: 'true' + serialized_type: '{"py/type": "builtins.bool"}' + inputs: + - key: TargetPoseInput.in + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + outputs: + - key: load_success + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.bool"}' + - key: load_error_msg + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + state: SHUTDOWN + - module: ros_bt_py.ros_nodes.subtree + node_class: Subtree + version: 0.2.0 + max_children: 0 + name: PutDownTrash + child_names: [] + options: + - key: subtree_path + serialized_value: '"package://ros_bt_py_pyrobosim/trees/put_down.yaml"' + serialized_type: '{"py/type": "builtins.str"}' + - key: use_io_nodes + serialized_value: 'true' + serialized_type: '{"py/type": "builtins.bool"}' + inputs: + - key: ObjectToPickup.in + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + outputs: + - key: load_success + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.bool"}' + - key: load_error_msg + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + state: SHUTDOWN +data_wirings: + - source: + node_name: WasteComponentName + data_kind: outputs + data_key: constant + target: + node_name: DetectObjects + data_kind: inputs + data_key: target_object + - source: + node_name: DetectObjects + data_kind: outputs + data_key: result_execution_result + target: + node_name: GetDetectObjectsResult + data_kind: inputs + data_key: in + - source: + node_name: EnumExeuctionResult + data_kind: outputs + data_key: SUCCESS + target: + node_name: DetectObjectResultSuccessfull? + data_kind: inputs + data_key: b + - source: + node_name: GetDetectObjectsResult + data_kind: outputs + data_key: status + target: + node_name: DetectObjectResultSuccessfull? + data_kind: inputs + data_key: a + - source: + node_name: DiningTrashHallway + data_kind: outputs + data_key: constant + target: + node_name: MoveToDiningTrashHallway + data_kind: inputs + data_key: TargetPoseInput.in + - source: + node_name: MoveToDumpAction + data_kind: outputs + data_key: result_execution_result + target: + node_name: GetMoveToDumpActionResult + data_kind: inputs + data_key: in + - source: + node_name: GetMoveToDumpActionResult + data_kind: outputs + data_key: status + target: + node_name: MoveToDumpSuccessful? + data_kind: inputs + data_key: b + - source: + node_name: EnumExeuctionResult + data_kind: outputs + data_key: SUCCESS + target: + node_name: MoveToDumpSuccessful? + data_kind: inputs + data_key: a + - source: + node_name: DetectObjects + data_kind: outputs + data_key: result_detected_objects + target: + node_name: GetConstListItem + data_kind: inputs + data_key: list + - source: + node_name: GetConstListItem + data_kind: outputs + data_key: item + target: + node_name: MessageToFields + data_kind: inputs + data_key: in + - source: + node_name: TrashLocations + data_kind: outputs + data_key: constant + target: + node_name: IterateList + data_kind: inputs + data_key: list + - source: + node_name: IterateList + data_kind: outputs + data_key: list_item + target: + node_name: MoveToTrashLocation + data_kind: inputs + data_key: TargetPoseInput.in + - source: + node_name: MessageToFields + data_kind: outputs + data_key: name + target: + node_name: PickUpTrash + data_kind: inputs + data_key: ObjectToPickUp.in + - source: + node_name: Dumpster + data_kind: outputs + data_key: constant + target: + node_name: PlanPathToDump + data_kind: inputs + data_key: TargetLocationIO.in + - source: + node_name: PlanPathToDump + data_kind: outputs + data_key: FoundPath.out + target: + node_name: MoveToDumpAction + data_kind: inputs + data_key: path + - source: + node_name: Dumpster + data_kind: outputs + data_key: constant + target: + node_name: PlanPathToDump_2 + data_kind: inputs + data_key: TargetLocationIO.in + - source: + node_name: PlanPathToDump_2 + data_kind: outputs + data_key: FoundPath.out + target: + node_name: MoveToDumpAction + data_kind: inputs + data_key: path + - source: + node_name: Dumpster + data_kind: outputs + data_key: constant + target: + node_name: MoveToDumpster_2 + data_kind: inputs + data_key: TargetPoseInput.in + - source: + node_name: MessageToFields + data_kind: outputs + data_key: name + target: + node_name: PutDownTrash + data_kind: inputs + data_key: ObjectToPickup.in +tick_frequency_hz: 10 +state: EDITABLE +public_node_data: + - node_name: EnumExeuctionResult + data_kind: outputs + data_key: CANCELED + - node_name: EnumExeuctionResult + data_kind: outputs + data_key: EXECUTION_FAILURE + - node_name: EnumExeuctionResult + data_kind: outputs + data_key: INVALID_ACTION + - node_name: EnumExeuctionResult + data_kind: outputs + data_key: PLANNING_FAILURE + - node_name: EnumExeuctionResult + data_kind: outputs + data_key: POSTCONDITION_FAILURE + - node_name: EnumExeuctionResult + data_kind: outputs + data_key: PRECONDITION_FAILURE + - node_name: EnumExeuctionResult + data_kind: outputs + data_key: STATUS__DEFAULT + - node_name: EnumExeuctionResult + data_kind: outputs + data_key: UNKNOWN + - node_name: Table + data_kind: outputs + data_key: constant + - node_name: PlanPathToDump + data_kind: outputs + data_key: load_success + - node_name: PlanPathToDump + data_kind: outputs + data_key: load_error_msg + - node_name: MoveToDiningTrashHallway + data_kind: outputs + data_key: load_success + - node_name: MoveToDiningTrashHallway + data_kind: outputs + data_key: load_error_msg + - node_name: OpenDoor + data_kind: outputs + data_key: load_success + - node_name: OpenDoor + data_kind: outputs + data_key: load_error_msg + - node_name: PlanPathToDump_2 + data_kind: outputs + data_key: load_success + - node_name: PlanPathToDump_2 + data_kind: outputs + data_key: load_error_msg + - node_name: GetMoveToDumpActionResult + data_kind: outputs + data_key: message + - node_name: OpenDumpster + data_kind: outputs + data_key: load_success + - node_name: OpenDumpster + data_kind: outputs + data_key: load_error_msg + - node_name: MoveToTrashLocation + data_kind: outputs + data_key: load_success + - node_name: MoveToTrashLocation + data_kind: outputs + data_key: load_error_msg + - node_name: GetDetectObjectsResult + data_kind: outputs + data_key: message + - node_name: MessageToFields + data_kind: outputs + data_key: category + - node_name: MessageToFields + data_kind: outputs + data_key: parent + - node_name: MessageToFields + data_kind: outputs + data_key: pose + - node_name: PickUpTrash + data_kind: outputs + data_key: load_success + - node_name: PickUpTrash + data_kind: outputs + data_key: load_error_msg + - node_name: MoveToDumpster_2 + data_kind: outputs + data_key: load_success + - node_name: MoveToDumpster_2 + data_kind: outputs + data_key: load_error_msg + - node_name: PutDownTrash + data_kind: outputs + data_key: load_success + - node_name: PutDownTrash + data_kind: outputs + data_key: load_error_msg diff --git a/technologies/ros_bt_py/ros_bt_py_pyrobosim/trees/put_down.yaml b/technologies/ros_bt_py/ros_bt_py_pyrobosim/trees/put_down.yaml new file mode 100644 index 0000000..c31fd23 --- /dev/null +++ b/technologies/ros_bt_py/ros_bt_py_pyrobosim/trees/put_down.yaml @@ -0,0 +1,286 @@ +name: '' +path: '' +root_name: .TempPutDownObjectSequence +nodes: + - module: ros_bt_py.nodes.sequence + node_class: MemorySequence + version: 0.1.0 + max_children: -1 + name: .TempPutDownObjectSequence + child_names: + - ObjectToPickup + - .EnumExeuctionResult + - .PlaceTaskAction + - .SetAttr + - .PutDownObject + - .PutDownResult + - .PutDownSuccessfull? + options: [] + inputs: [] + outputs: [] + state: SHUTDOWN + - module: ros_bt_py.nodes.io + node_class: IOInputOption + version: 0.1.0 + max_children: 0 + name: ObjectToPickup + child_names: [] + options: + - key: io_type + serialized_value: '{"py/type": "builtins.str"}' + serialized_type: '{"py/type": "builtins.type"}' + - key: default + serialized_value: '"pick_obj"' + serialized_type: '{"py/type": "builtins.str"}' + inputs: + - key: in + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + outputs: + - key: out + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + state: SHUTDOWN + - module: ros_bt_py.ros_nodes.enum + node_class: EnumFields + version: 0.1.0 + max_children: 0 + name: .EnumExeuctionResult + child_names: [] + options: + - key: ros_message_type + serialized_value: '{"py/type": "pyrobosim_msgs.msg._execution_result.ExecutionResult"}' + serialized_type: '{"py/type": "builtins.type"}' + inputs: [] + outputs: + - key: CANCELED + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: EXECUTION_FAILURE + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: INVALID_ACTION + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: PLANNING_FAILURE + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: POSTCONDITION_FAILURE + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: PRECONDITION_FAILURE + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: STATUS__DEFAULT + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: SUCCESS + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: UNKNOWN + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + state: SHUTDOWN + - module: ros_bt_py.ros_nodes.messages_from_dict + node_class: MessageFromConstDict + version: 0.9.0 + max_children: 0 + name: .PlaceTaskAction + child_names: [] + options: + - key: message_type + serialized_value: '{"py/type": "pyrobosim_msgs.msg._task_action.TaskAction"}' + serialized_type: '{"py/type": "builtins.type"}' + - key: dict + serialized_value: '{"robot": "robot", "type": "place"}' + serialized_type: '{"py/type": "builtins.dict"}' + inputs: [] + outputs: + - key: message + serialized_value: 'null' + serialized_type: '{"py/type": "pyrobosim_msgs.msg._task_action.TaskAction"}' + state: SHUTDOWN + - module: ros_bt_py.nodes.setters + node_class: SetAttr + version: 0.1.0 + max_children: 0 + name: .SetAttr + child_names: [] + options: + - key: object_type + serialized_value: '{"py/type": "pyrobosim_msgs.msg._task_action.TaskAction"}' + serialized_type: '{"py/type": "builtins.type"}' + - key: attr_name + serialized_value: '"object"' + serialized_type: '{"py/type": "builtins.str"}' + - key: attr_type + serialized_value: '{"py/type": "builtins.str"}' + serialized_type: '{"py/type": "builtins.type"}' + inputs: + - key: object + serialized_value: 'null' + serialized_type: '{"py/type": "pyrobosim_msgs.msg._task_action.TaskAction"}' + - key: attr_value + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + outputs: + - key: new_object + serialized_value: 'null' + serialized_type: '{"py/type": "pyrobosim_msgs.msg._task_action.TaskAction"}' + state: SHUTDOWN + - module: ros_bt_py.ros_nodes.action + node_class: Action + version: 0.1.0 + max_children: 0 + name: .PutDownObject + child_names: [] + options: + - key: action_type + serialized_value: >- + {"py/type": + "pyrobosim_msgs.action._execute_task_action.ExecuteTaskAction"} + serialized_type: '{"py/type": "builtins.type"}' + - key: action_name + serialized_value: '"/execute_action"' + serialized_type: '{"py/type": "builtins.str"}' + - key: wait_for_action_server_seconds + serialized_value: '2.0' + serialized_type: '{"py/type": "builtins.float"}' + - key: timeout_seconds + serialized_value: '5.0' + serialized_type: '{"py/type": "builtins.float"}' + - key: fail_if_not_available + serialized_value: 'true' + serialized_type: '{"py/type": "builtins.bool"}' + inputs: + - key: action + serialized_value: 'null' + serialized_type: '{"py/type": "pyrobosim_msgs.msg._task_action.TaskAction"}' + outputs: + - key: result_execution_result + serialized_value: 'null' + serialized_type: '{"py/type": "pyrobosim_msgs.msg._execution_result.ExecutionResult"}' + state: SHUTDOWN + - module: ros_bt_py.ros_nodes.message_converters + node_class: MessageToFields + version: 0.1.0 + max_children: 0 + name: .PutDownResult + child_names: [] + options: + - key: input_type + serialized_value: '{"py/type": "pyrobosim_msgs.msg._execution_result.ExecutionResult"}' + serialized_type: '{"py/type": "builtins.type"}' + inputs: + - key: in + serialized_value: 'null' + serialized_type: '{"py/type": "pyrobosim_msgs.msg._execution_result.ExecutionResult"}' + outputs: + - key: status + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: message + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.str"}' + state: SHUTDOWN + - module: ros_bt_py.nodes.compare + node_class: Compare + version: 0.1.0 + max_children: 0 + name: .PutDownSuccessfull? + child_names: [] + options: + - key: compare_type + serialized_value: '{"py/type": "builtins.int"}' + serialized_type: '{"py/type": "builtins.type"}' + inputs: + - key: a + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + - key: b + serialized_value: 'null' + serialized_type: '{"py/type": "builtins.int"}' + outputs: [] + state: SHUTDOWN +data_wirings: + - source: + node_name: .PlaceTaskAction + data_kind: outputs + data_key: message + target: + node_name: .SetAttr + data_kind: inputs + data_key: object + - source: + node_name: .SetAttr + data_kind: outputs + data_key: new_object + target: + node_name: .PutDownObject + data_kind: inputs + data_key: action + - source: + node_name: .PutDownObject + data_kind: outputs + data_key: result_execution_result + target: + node_name: .PutDownResult + data_kind: inputs + data_key: in + - source: + node_name: .PutDownResult + data_kind: outputs + data_key: status + target: + node_name: .PutDownSuccessfull? + data_kind: inputs + data_key: b + - source: + node_name: .EnumExeuctionResult + data_kind: outputs + data_key: SUCCESS + target: + node_name: .PutDownSuccessfull? + data_kind: inputs + data_key: a + - source: + node_name: ObjectToPickup + data_kind: outputs + data_key: out + target: + node_name: .SetAttr + data_kind: inputs + data_key: attr_value +tick_frequency_hz: 10 +state: EDITABLE +public_node_data: + - node_name: ObjectToPickup + data_kind: inputs + data_key: in + - node_name: .EnumExeuctionResult + data_kind: outputs + data_key: CANCELED + - node_name: .EnumExeuctionResult + data_kind: outputs + data_key: EXECUTION_FAILURE + - node_name: .EnumExeuctionResult + data_kind: outputs + data_key: INVALID_ACTION + - node_name: .EnumExeuctionResult + data_kind: outputs + data_key: PLANNING_FAILURE + - node_name: .EnumExeuctionResult + data_kind: outputs + data_key: POSTCONDITION_FAILURE + - node_name: .EnumExeuctionResult + data_kind: outputs + data_key: PRECONDITION_FAILURE + - node_name: .EnumExeuctionResult + data_kind: outputs + data_key: STATUS__DEFAULT + - node_name: .EnumExeuctionResult + data_kind: outputs + data_key: UNKNOWN + - node_name: .PutDownResult + data_kind: outputs + data_key: message From a33eaeb001fb2497ae46df778d688f9a412dc959 Mon Sep 17 00:00:00 2001 From: David Conner Date: Wed, 2 Oct 2024 20:20:54 -0400 Subject: [PATCH 45/51] merge PR #40 with additional change to CMakeLists.txt to export include files --- dependencies/BehaviorTree.ROS2 | 2 +- .../pyrobosim_btcpp/CMakeLists.txt | 7 +++- .../pyrobosim_btcpp/nodes/close_door_node.hpp | 33 ++++++++++++++++ .../nodes/detect_object_node.hpp | 38 +++++++++++++++++++ .../nodes/execute_task_node.hpp | 4 ++ .../pyrobosim_btcpp/nodes/navigate_node.hpp | 2 + .../pyrobosim_btcpp/nodes/open_door_node.hpp | 33 ++++++++++++++++ .../nodes/pick_object_node.hpp | 38 +++++++++++++++++++ .../nodes/place_object_node.hpp | 38 +++++++++++++++++++ .../pyrobosim_btcpp/src/btcpp_executor.cpp | 10 +++++ .../pyrobosim_btcpp/trees/problem1.xml | 24 ++++++++++++ .../pyrobosim_btcpp/trees/problem2.xml | 17 +++++++++ 12 files changed, 244 insertions(+), 2 deletions(-) create mode 100644 technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/problem1.xml create mode 100644 technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/problem2.xml diff --git a/dependencies/BehaviorTree.ROS2 b/dependencies/BehaviorTree.ROS2 index adec04b..cc31ea7 160000 --- a/dependencies/BehaviorTree.ROS2 +++ b/dependencies/BehaviorTree.ROS2 @@ -1 +1 @@ -Subproject commit adec04ba58531de37f87328632a72a506cc6b0d4 +Subproject commit cc31ea7b97947f1aac6e8c37df6cec379c84a7d9 diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/CMakeLists.txt b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/CMakeLists.txt index bd47242..b924281 100644 --- a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/CMakeLists.txt +++ b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/CMakeLists.txt @@ -38,7 +38,12 @@ install( install( TARGETS btcpp_executor - DESTINATION DESTINATION lib/${PROJECT_NAME} + DESTINATION lib/${PROJECT_NAME} ) +install(DIRECTORY include/ DESTINATION include/) + +ament_export_dependencies(${THIS_PACKAGE_DEPS}) +ament_export_include_directories(include) + ament_package() diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/close_door_node.hpp b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/close_door_node.hpp index e69de29..adf186a 100644 --- a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/close_door_node.hpp +++ b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/close_door_node.hpp @@ -0,0 +1,33 @@ +#pragma once + +#include +#include +#include "execute_task_node.hpp" + +namespace BT +{ + +class CloseDoorAction : public ExecuteTaskNode +{ +public: + CloseDoorAction(const std::string& name, const NodeConfig& conf, const RosNodeParams& params) + : ExecuteTaskNode(name, conf, params) + {} + + // specify the ports offered by this node + static BT::PortsList providedPorts() + { + return ExecuteTaskNode::appendProvidedPorts({}); + } + + // Implement the method that sends the goal + bool setGoal(TaskAction& action) override + { + // prepare the goal message + action.type = "close"; + action.target_location = "door"; + return true; + } +}; + +} // namespace BT diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/detect_object_node.hpp b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/detect_object_node.hpp index e69de29..e4156d5 100644 --- a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/detect_object_node.hpp +++ b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/detect_object_node.hpp @@ -0,0 +1,38 @@ +#pragma once + +#include +#include +#include "execute_task_node.hpp" + +namespace BT +{ + +class DetectObject : public ExecuteTaskNode +{ +public: + DetectObject(const std::string& name, const NodeConfig& conf, const RosNodeParams& params) + : ExecuteTaskNode(name, conf, params) + {} + + // specify the ports offered by this node + static BT::PortsList providedPorts() + { + return ExecuteTaskNode::appendProvidedPorts({ BT::InputPort("object") }); + } + + // Implement the method that sends the goal + bool setGoal(TaskAction& action) override + { + std::string object; + if(!getInput("object", object) || object.empty()) + { + throw BT::RuntimeError("missing required input [object]"); + } + // prepare the goal message + action.type = "detect"; + action.object = object; + return true; + } +}; + +} // namespace BT diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/execute_task_node.hpp b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/execute_task_node.hpp index 562515a..dc549a6 100644 --- a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/execute_task_node.hpp +++ b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/execute_task_node.hpp @@ -1,3 +1,5 @@ +#pragma once + #include #include @@ -80,6 +82,8 @@ inline NodeStatus ExecuteTaskNode::onResultReceived(const ExecutionResult& execu resultToStr(execution_result), execution_result.message.c_str()); return NodeStatus::FAILURE; } + RCLCPP_INFO(logger(), "[%s] succeeded. Message: %s", name().c_str(), + execution_result.message.c_str()); return NodeStatus::SUCCESS; } diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/navigate_node.hpp b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/navigate_node.hpp index 1e0e629..ddbee83 100644 --- a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/navigate_node.hpp +++ b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/navigate_node.hpp @@ -1,3 +1,5 @@ +#pragma once + #include #include #include "execute_task_node.hpp" diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/open_door_node.hpp b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/open_door_node.hpp index e69de29..1ac4ff1 100644 --- a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/open_door_node.hpp +++ b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/open_door_node.hpp @@ -0,0 +1,33 @@ +#pragma once + +#include +#include +#include "execute_task_node.hpp" + +namespace BT +{ + +class OpenDoorAction : public ExecuteTaskNode +{ +public: + OpenDoorAction(const std::string& name, const NodeConfig& conf, const RosNodeParams& params) + : ExecuteTaskNode(name, conf, params) + {} + + // specify the ports offered by this node + static BT::PortsList providedPorts() + { + return ExecuteTaskNode::appendProvidedPorts({}); + } + + // Implement the method that sends the goal + bool setGoal(TaskAction& action) override + { + // prepare the goal message + action.type = "open"; + action.target_location = "door"; + return true; + } +}; + +} // namespace BT diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/pick_object_node.hpp b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/pick_object_node.hpp index e69de29..f40c32f 100644 --- a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/pick_object_node.hpp +++ b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/pick_object_node.hpp @@ -0,0 +1,38 @@ +#pragma once + +#include +#include +#include "execute_task_node.hpp" + +namespace BT +{ + +class PickObject : public ExecuteTaskNode +{ +public: + PickObject(const std::string& name, const NodeConfig& conf, const RosNodeParams& params) + : ExecuteTaskNode(name, conf, params) + {} + + // specify the ports offered by this node + static BT::PortsList providedPorts() + { + return ExecuteTaskNode::appendProvidedPorts({ BT::InputPort("object") }); + } + + // Implement the method that sends the goal + bool setGoal(TaskAction& action) override + { + std::string object; + if(!getInput("object", object) || object.empty()) + { + throw BT::RuntimeError("missing required input [object]"); + } + // prepare the goal message + action.type = "pick"; + action.object = object; + return true; + } +}; + +} // namespace BT diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/place_object_node.hpp b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/place_object_node.hpp index e69de29..7c9d8fc 100644 --- a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/place_object_node.hpp +++ b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/place_object_node.hpp @@ -0,0 +1,38 @@ +#pragma once + +#include +#include +#include "execute_task_node.hpp" + +namespace BT +{ + +class PlaceObject : public ExecuteTaskNode +{ +public: + PlaceObject(const std::string& name, const NodeConfig& conf, const RosNodeParams& params) + : ExecuteTaskNode(name, conf, params) + {} + + // specify the ports offered by this node + static BT::PortsList providedPorts() + { + return ExecuteTaskNode::appendProvidedPorts({ BT::InputPort("object") }); + } + + // Implement the method that sends the goal + bool setGoal(TaskAction& action) override + { + std::string object; + if(!getInput("object", object) || object.empty()) + { + throw BT::RuntimeError("missing required input [object]"); + } + // prepare the goal message + action.type = "place"; + action.object = object; + return true; + } +}; + +} // namespace BT diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/src/btcpp_executor.cpp b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/src/btcpp_executor.cpp index cfc32d5..1798e0e 100644 --- a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/src/btcpp_executor.cpp +++ b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/src/btcpp_executor.cpp @@ -10,7 +10,12 @@ #include // BTCPP nodes in this package +#include "pyrobosim_btcpp/nodes/open_door_node.hpp" +#include "pyrobosim_btcpp/nodes/close_door_node.hpp" +#include "pyrobosim_btcpp/nodes/detect_object_node.hpp" #include "pyrobosim_btcpp/nodes/navigate_node.hpp" +#include "pyrobosim_btcpp/nodes/pick_object_node.hpp" +#include "pyrobosim_btcpp/nodes/place_object_node.hpp" std::filesystem::path GetFilePath(const std::string& filename) { @@ -59,7 +64,12 @@ int main(int argc, char** argv) params.nh = nh; params.default_port_value = "execute_action"; + factory.registerNodeType("CloseDoor", params); + factory.registerNodeType("DetectObject", params); factory.registerNodeType("Navigate", params); + factory.registerNodeType("OpenDoor", params); + factory.registerNodeType("PickObject", params); + factory.registerNodeType("PlaceObject", params); // optionally we can display and save the model of the tree if(save_model) diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/problem1.xml b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/problem1.xml new file mode 100644 index 0000000..b04a77c --- /dev/null +++ b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/problem1.xml @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/problem2.xml b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/problem2.xml new file mode 100644 index 0000000..967bcc3 --- /dev/null +++ b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/problem2.xml @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + From 981ef5ab2fdc08059a9455dda803bedfc4753db0 Mon Sep 17 00:00:00 2001 From: David Conner Date: Wed, 2 Oct 2024 20:21:35 -0400 Subject: [PATCH 46/51] update flexbe_webui target to handle customizable license --- dependencies/FlexBE/flexbe_webui | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dependencies/FlexBE/flexbe_webui b/dependencies/FlexBE/flexbe_webui index 453b7a7..fc7c927 160000 --- a/dependencies/FlexBE/flexbe_webui +++ b/dependencies/FlexBE/flexbe_webui @@ -1 +1 @@ -Subproject commit 453b7a7b9c44d138796e467f650de07264c771c0 +Subproject commit fc7c9276a0824da6a2342fc3040f9eaf2fc31bef From 3d3a8f0936a644985a9aaf47ad2dddab82e9f0b8 Mon Sep 17 00:00:00 2001 From: David Conner Date: Wed, 2 Oct 2024 20:24:31 -0400 Subject: [PATCH 47/51] modify to use BSD-3 clause license; add run_btcpp_tree_state and pyrobosim_flexbe_btcpp executor; add HFSMBTH behavior; add copy of BT.cpp demo trees and change to unique tree name required by executor --- technologies/FlexBE/CONTRIBUTING.md | 4 +- technologies/FlexBE/LICENSE | 33 +- technologies/FlexBE/README.md | 19 +- technologies/FlexBE/docs/hfsmbth.md | 19 ++ .../pyrobosim_flexbe_behaviors/CMakeLists.txt | 5 - .../bin/copy_behavior | 100 ------- .../manifest/btcpphfsmbth.xml | 18 ++ .../manifest/go_beh.xml | 2 +- .../manifest/patrol.xml | 2 +- .../manifest/patrolcharge.xml | 2 +- .../manifest/through_door.xml | 2 +- .../manifest/traverse.xml | 2 +- .../pyrobosim_flexbe_behaviors/package.xml | 2 +- .../btcpphfsmbth_sm.py | 215 +++++++++++++ .../delib_ws_p2_sm.py | 31 +- .../delib_ws_p2_sm_sm.py | 31 +- .../detectselect_sm.py | 31 +- .../pyrobosim_flexbe_behaviors/go_beh_sm.py | 35 ++- .../pyrobosim_flexbe_behaviors/patrol_sm.py | 35 ++- .../patrolcharge_sm.py | 35 ++- .../test_navigate_sm.py | 31 +- .../test_pick_place_sm.py | 31 +- .../test_plan_path_sm.py | 31 +- .../through_door_sm.py | 35 ++- .../pyrobosim_flexbe_behaviors/traverse_sm.py | 35 ++- .../pyrobosim_flexbe_behaviors/setup.py | 2 +- .../pyrobosim_flexbe_btcpp/.clang-format | 68 +++++ .../pyrobosim_flexbe_btcpp/CMakeLists.txt | 51 ++++ .../config/pyrobosim_flexbe_btcpp.yaml | 13 + .../launch/pyrobosim_flexbe_btcpp.launch.xml | 5 + .../FlexBE/pyrobosim_flexbe_btcpp/package.xml | 38 +++ .../src/flexbe_btcpp_executor.cpp | 283 ++++++++++++++++++ .../trees/navigation_demo.xml | 11 + .../trees/office_nav.xml | 9 + .../pyrobosim_flexbe_btcpp/trees/problem1.xml | 24 ++ .../pyrobosim_flexbe_btcpp/trees/problem2.xml | 17 ++ .../pyrobosim_flexbe_states/package.xml | 2 +- .../check_door_state.py | 31 +- .../detect_local_objects_state.py | 31 +- .../detect_objects_state.py | 31 +- .../door_action_state.py | 31 +- .../follow_path_state.py | 33 +- .../monitor_battery_state.py | 31 +- .../navigate_action_state.py | 31 +- .../next_room_state.py | 31 +- .../pick_action_state.py | 31 +- .../place_action_state.py | 31 +- .../plan_path_state.py | 33 +- .../run_btcpp_tree_state.py | 205 +++++++++++++ .../FlexBE/pyrobosim_flexbe_states/setup.py | 2 +- .../pyrobosim_flexbe_utilities/package.xml | 2 +- .../world_configuration.py | 31 +- .../pyrobosim_flexbe_utilities/setup.py | 2 +- 53 files changed, 1548 insertions(+), 348 deletions(-) create mode 100644 technologies/FlexBE/docs/hfsmbth.md delete mode 100755 technologies/FlexBE/pyrobosim_flexbe_behaviors/bin/copy_behavior create mode 100644 technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/btcpphfsmbth.xml create mode 100644 technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/btcpphfsmbth_sm.py create mode 100644 technologies/FlexBE/pyrobosim_flexbe_btcpp/.clang-format create mode 100644 technologies/FlexBE/pyrobosim_flexbe_btcpp/CMakeLists.txt create mode 100644 technologies/FlexBE/pyrobosim_flexbe_btcpp/config/pyrobosim_flexbe_btcpp.yaml create mode 100644 technologies/FlexBE/pyrobosim_flexbe_btcpp/launch/pyrobosim_flexbe_btcpp.launch.xml create mode 100644 technologies/FlexBE/pyrobosim_flexbe_btcpp/package.xml create mode 100644 technologies/FlexBE/pyrobosim_flexbe_btcpp/src/flexbe_btcpp_executor.cpp create mode 100644 technologies/FlexBE/pyrobosim_flexbe_btcpp/trees/navigation_demo.xml create mode 100644 technologies/FlexBE/pyrobosim_flexbe_btcpp/trees/office_nav.xml create mode 100644 technologies/FlexBE/pyrobosim_flexbe_btcpp/trees/problem1.xml create mode 100644 technologies/FlexBE/pyrobosim_flexbe_btcpp/trees/problem2.xml create mode 100644 technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/run_btcpp_tree_state.py diff --git a/technologies/FlexBE/CONTRIBUTING.md b/technologies/FlexBE/CONTRIBUTING.md index 0703ae9..3665149 100644 --- a/technologies/FlexBE/CONTRIBUTING.md +++ b/technologies/FlexBE/CONTRIBUTING.md @@ -1,3 +1,3 @@ Any new contributions that you make to this repository will -be under the Apache 2.0 License, as dictated by that -[license](http://www.apache.org/licenses/LICENSE-2.0). +be under the BSD-3 License, as dictated by that +[license](https://opensource.org/license/bsd-3-clause). diff --git a/technologies/FlexBE/LICENSE b/technologies/FlexBE/LICENSE index d809660..b147768 100644 --- a/technologies/FlexBE/LICENSE +++ b/technologies/FlexBE/LICENSE @@ -1,13 +1,26 @@ -Copyright 2023 Philipp Schillinger, Christopher Newport University +Copyright 2024 Christopher Newport University -Licensed under the Apache License, Version 2.0 (the "License"); -you may not use this file except in compliance with the License. -You may obtain a copy of the License at +Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: - http://www.apache.org/licenses/LICENSE-2.0 + 1. Redistributions of source code must retain the above copyright notice, + this list of conditions and the following disclaimer. -Unless required by applicable law or agreed to in writing, software -distributed under the License is distributed on an "AS IS" BASIS, -WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -See the License for the specific language governing permissions and -limitations under the License. + 2. 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. + + 3. 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/technologies/FlexBE/README.md b/technologies/FlexBE/README.md index de84c4e..3c1c09b 100644 --- a/technologies/FlexBE/README.md +++ b/technologies/FlexBE/README.md @@ -35,7 +35,6 @@ To run start the following in separate terminals > `export QT_QUICK_BACKEND=software` in terminal or use the aliased helper function > `qt_soft_render()` before launching the `webui_client` - * `clear; ros2 run flexbe_input input_action_server` * This launches an action server that will pop up a simple UI that will allow the operator input data on request * In this demonstration, it is used to select from among the detected objects @@ -92,6 +91,7 @@ The `pyrobosim_flexbe_states` package under `technologies/FlexBE` includes: * `pick_action_state.py` - Does pick action at current location * `place_action_state.py` - Does place action at current location * `plan_path_state.py` - Request plan from current location to target location +* `run_btcpp_tree_state.py` - Run a behavior tree using custom BehaviorTree.CPP v4 executor The above states have extra logging information that is shown in the onboard behavior terminal. Each state transition `on_start`, `on_enter`, `on_exit`, `on_pause`, `on_resume`, and `on_stop` are logged. This is not recommended in regular states, but is done here for educational purposes. @@ -125,3 +125,20 @@ Use `clear; ros2 run delib_ws_worlds run --ros-args -p problem_number:=1` * This pauses the `Patrol` behavior until finished charging. On resuming, any active planning or follow states will return `failed` and restart the `Patrol` behavior. You can build on these behavior demonstrations to solve the workshop tasks. + + +See ["Mythical HFSMBT Hybrid"](docs/hfsmbth.md) for an example demonstration of a hybrid HFSM/BT. + + +## FlexBE Publications + +Please use the following publications for reference when using FlexBE and the FlexBE WebUI: + +- Philipp Schillinger, Stefan Kohlbrecher, and Oskar von Stryk, ["Human-Robot Collaborative High-Level Control with Application to Rescue Robotics"](http://dx.doi.org/10.1109/ICRA.2016.7487442), IEEE International Conference on Robotics and Automation (ICRA), Stockholm, Sweden, May 2016. + +- Joshua Zutell, David C. Conner, and Philipp Schillinger, ["ROS 2-Based Flexible Behavior Engine for Flexible Navigation"](http://dx.doi.org/10.1109/SoutheastCon48659.2022.9764047), IEEE SouthEastCon, April 2022. + +- Joshua Zutell, David C. Conner, and Philipp Schillinger, ["Flexible Behavior Trees: In search of the mythical HFSMBTH for Collaborative Autonomy in Robotics"](https://doi.org/10.48550/arXiv.2203.05389), 2022. + +- Samuel Raymond, Grace Walters, Joshua Luzier, and David C. Conner, "Design and Development of the FlexBE WebUI with Introductory Tutorials", J. Comput. Sci. Coll vol.40, no.3, CCSC Eastern, to appear October 2024. + diff --git a/technologies/FlexBE/docs/hfsmbth.md b/technologies/FlexBE/docs/hfsmbth.md new file mode 100644 index 0000000..da91794 --- /dev/null +++ b/technologies/FlexBE/docs/hfsmbth.md @@ -0,0 +1,19 @@ +## The Mythical HFSMBTH + +Inspired by the 2019 Game Developers Conference [talk](https://www.youtube.com/watch?v=Qq_xX1JCreI&t=1159s) by Bobby Anguelov, we present the "Mythical HFSMBT Hybrid". + +Please use the following publication for reference when using the HFSMBTH: + + * Joshua M. Zutell, David C. Conner, and Philipp Schillinger, "Flexible Behavior Trees: In search of the mythical HFSMBTH for Collaborative Autonomy in Robotics", arXiv 2022, https://doi.org/10.48550/arXiv.2203.05389. + + +#### BehaviorTree.cpp Hybrid Demo + + * `clear; ros2 run delib_ws_worlds run --ros-args -p problem_number:=2` + * `clear; ros2 launch flexbe_onboard behavior_onboard.launch.py` + * `clear; ros2 launch flexbe_webui flexbe_ocs.launch.py headless:=true` + * `clear; ros2 run flexbe_webui webui_client` + * `clear; ros2 launch pyrobosim_flexbe_btcpp pyrobosim_flexbe_btcpp.launch.xml` + + And load `BtCppHFSMBTH` behavior. + This uses a `RunBtCppTreeState` and a custom `flexbe_btcpp_executor` node. diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/CMakeLists.txt b/technologies/FlexBE/pyrobosim_flexbe_behaviors/CMakeLists.txt index d2412a5..83c65a7 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_behaviors/CMakeLists.txt +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/CMakeLists.txt @@ -15,11 +15,6 @@ install(DIRECTORY DESTINATION lib/${PROJECT_NAME} ) -install(PROGRAMS - bin/copy_behavior - DESTINATION lib/${PROJECT_NAME} -) - # Install Python modules ament_python_install_package(${PROJECT_NAME}) diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/bin/copy_behavior b/technologies/FlexBE/pyrobosim_flexbe_behaviors/bin/copy_behavior deleted file mode 100755 index 85da3e0..0000000 --- a/technologies/FlexBE/pyrobosim_flexbe_behaviors/bin/copy_behavior +++ /dev/null @@ -1,100 +0,0 @@ -#!/bin/bash - -# Copyright 2023 Christopher Newport University -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - - -# WARNING: Use at your own risk, this script does not protect against overwriting files, or mis-naming - -if [ $# -lt 1 ]; then - echo -e "\e[93mThis is an extremely simple script designed to copy behaviors\033[0m" - echo -e "\e[93mfrom the install folder to the src folder assumed to be a git repo\033[0m" - echo -e "\e[93mfor storage.\033[0m" - echo "" - echo -e "\e[93mIt requires a WORKSPACE_ROOT environment variable assuming a standard \033[0m" - echo -e "\e[93m \${WORKSPACE_ROOT}/install and \${WORKSPACE_ROOT}/src layout.\033[0m" - echo "" - echo -e "\e[93mRun this script from the base src folder for behaviors package\033[0m" - echo -e "\e[93m e.g., \${WORKSPACE_ROOT}/src/pyrobosim_flexbe\033[0m" - echo "" - echo -e "\e[93m Usage: ros2 run pyrobosim_flexbe_behaviors copy_behavior BEHAVIOR_FILE_BASE \033[0m" - echo -e "\e[93m where BEHAVIOR_FILE_BASE_NAME.xml is the saved manifest name\033[0m" - echo -e "\e[93m OPTIONAL_BEHAVIOR_PACKAGE defaults to 'pyrobosim_flexbe_behaviors'\033[0m" - echo "" - echo -e "\e[93m WARNING: Use at your own risk, this script does not protect against overwriting files or mis-naming\033[0m" - exit 2 -fi - - -beh=$(echo "$1" | cut -f 1 -d '.') # include only the base file name is someone pastes .xml name -pack="pyrobosim_flexbe_behaviors" # Default name uses this behaviors package -if [ $# -eq 2 ]; then - pack="$2" - echo "Using specified package '${pack}'!" -fi - -# A few basic checks before attempting to copy -if [ ! -d "${pack}" ]; then - echo "" - echo -e "\e[91m Package '${pack}' does not exist under current directory '${PWD}' !\033[0m" - echo "" - echo -e "\e[93mRun this script from the base src folder for behaviors package\033[0m" - echo -e "\e[93m e.g., \${WORKSPACE_ROOT}/src/pyrobosim_flexbe\033[0m" - exit -fi - -if [ ! -d "${pack}/${pack}" ]; then - echo "" - echo -e "\e[91m Behavior implementation folder '${pack}/${pack}' does not exist under current directory '${PWD}' !\033[0m" - echo "" - echo -e "\e[93mRun this script from the base src folder for behaviors package\033[0m" - echo -e "\e[93m e.g., \${WORKSPACE_ROOT}/src/pyrobosim_flexbe\033[0m" - exit -fi - -if [ ! -f "${WORKSPACE_ROOT}/install/${pack}/local/lib/python3.10/dist-packages/${pack}/${beh}_sm.py" ]; then - echo "" - echo -e "\e[91m Behavior '${beh}' implementation does not exist in install folder for '${pack}' package!\033[0m" - echo "" - echo "Available behavior manifests in '${pack}' :" - ls ${WORKSPACE_ROOT}/install/${pack}/lib/${pack}/manifest - echo "" - echo -e "\e[93mConfirm behavior and package names!\033[0m" - exit -fi - - -# Begin the work -echo "Copying '${beh}' implementation to '${pack}' under '${PWD}' ..." -cp ${WORKSPACE_ROOT}/install/${pack}/local/lib/python3.10/dist-packages/${pack}/${beh}_sm.py ./${pack}/${pack}/${beh}_sm.py - -if test $? -eq 0; then - echo "Copying '${beh}.xml' manifest to '${pack}' ..." - - cp ${WORKSPACE_ROOT}/install/${pack}/lib/${pack}/manifest/${beh}.xml ./${pack}/manifest/${beh}.xml - if test $? -eq 0; then - echo "" - echo -e "\e[92mDone copying behavior '${beh}' to '${pack}'!\033[0m" - echo "Do not forget to git commit and git push any changes." - exit - else - echo "" - echo -e "\e[91m Failed to copy behavior '${beh}.xml' manifest after copying behavior implementation!\033[0m" - exit - fi -else - echo "" - echo -e "\e[91m Failed to copy behavior '${beh}'!\033[0m" - exit -fi diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/btcpphfsmbth.xml b/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/btcpphfsmbth.xml new file mode 100644 index 0000000..f17c253 --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/btcpphfsmbth.xml @@ -0,0 +1,18 @@ + + + + + + pyrobosim, hfsmbth, btcpp + David Conner + Sun Sep 29 2024 + + Demo of the mythical HFSMBTH using BehaviorTree.cpp + + + + + + + + diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/go_beh.xml b/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/go_beh.xml index a7e1504..d46514c 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/go_beh.xml +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/go_beh.xml @@ -4,7 +4,7 @@ pyrobosim, delib_ws - Conner + David Conner Sun Sep 08 2024 Go to target behavior diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/patrol.xml b/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/patrol.xml index 30a8db9..64b5edf 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/patrol.xml +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/patrol.xml @@ -4,7 +4,7 @@ pyrobosim, patrol - Conner + David Conner Mon Sep 23 2024 Patrol rooms without concern for battery diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/patrolcharge.xml b/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/patrolcharge.xml index 74660a8..95272eb 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/patrolcharge.xml +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/patrolcharge.xml @@ -4,7 +4,7 @@ pyrobosim, battery - Conner + David Conner Mon Sep 23 2024 Patrol and recharge as necessary diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/through_door.xml b/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/through_door.xml index 189c5f4..5e91775 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/through_door.xml +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/through_door.xml @@ -4,7 +4,7 @@ pyrobosim - conner + David Conner Sun Sep 08 2024 Go through a door, opening if required diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/traverse.xml b/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/traverse.xml index a2ffd25..0591180 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/traverse.xml +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/manifest/traverse.xml @@ -4,7 +4,7 @@ pyrobosim, delib_ws - conner + David Conner Sun Sep 08 2024 Traverse the world opening doors as required diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/package.xml b/technologies/FlexBE/pyrobosim_flexbe_behaviors/package.xml index a67c7e1..1dcd7c7 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_behaviors/package.xml +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/package.xml @@ -11,7 +11,7 @@ David Conner David Conner - Apache 2 + BSD-3 rclpy flexbe_behavior_engine diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/btcpphfsmbth_sm.py b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/btcpphfsmbth_sm.py new file mode 100644 index 0000000..48e9ba4 --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/btcpphfsmbth_sm.py @@ -0,0 +1,215 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +# Copyright 2024 David Conner +# +# Redistribution and use in source and binary forms, with or without modification, +# are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. + +# 2. 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. +# +# 3. 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. + +########################################################### +# WARNING: Generated code! # +# ************************** # +# Manual changes may get lost if file is generated again. # +# Only code inside the [MANUAL] tags will be kept. # +########################################################### + +""" +Define BtCppHFSMBTH. + +Demo of the mythical HFSMBTH using BehaviorTree.cpp + +Created on Sun Sep 29 2024 +@author: David Conner +""" + + +from flexbe_core import Autonomy +from flexbe_core import Behavior +from flexbe_core import ConcurrencyContainer +from flexbe_core import Logger +from flexbe_core import OperatableStateMachine +from flexbe_core import PriorityContainer +from flexbe_core import initialize_flexbe_core +from flexbe_states.log_key_state import LogKeyState +from flexbe_states.operator_decision_state import OperatorDecisionState +from pyrobosim_flexbe_states.run_btcpp_tree_state import RunBtCppState + +# Additional imports can be added inside the following tags +# [MANUAL_IMPORT] + + +# [/MANUAL_IMPORT] + + +class BtCppHFSMBTHSM(Behavior): + """ + Define BtCppHFSMBTH. + + Demo of the mythical HFSMBTH using BehaviorTree.cpp + """ + + def __init__(self, node): + super().__init__() + self.name = 'BtCppHFSMBTH' + + # parameters of this behavior + + # Initialize ROS node information + initialize_flexbe_core(node) + + # references to used behaviors + + # Additional initialization code can be added inside the following tags + # [MANUAL_INIT] + + + # [/MANUAL_INIT] + + # Behavior comments: + + def create(self): + """Create state machine.""" + # Root state machine + # x:722 y:62 + _state_machine = OperatableStateMachine(outcomes=['finished']) + _state_machine.userdata.nav_tree = 'NavigationDemoTree' + _state_machine.userdata.p1_tree = 'Problem1Tree' + _state_machine.userdata.p2_tree = 'Problem2Tree' + _state_machine.userdata.bt_payload = '' + _state_machine.userdata.office_tree = 'OfficeNavTree' + + # Additional creation code can be added inside the following tags + # [MANUAL_CREATE] + + + # [/MANUAL_CREATE] + + with _state_machine: + # x:53 y:37 + OperatableStateMachine.add('ChooseTree', + OperatorDecisionState(outcomes=['nav', + 'p1', + 'p2', + 'quit', + 'office'], + hint="Choose a behavior tree", + suggestion='nav'), + transitions={'nav': 'NavTree' # 185 213 139 90 -1 -1 + , 'p1': 'P1Tree' # 186 316 123 90 -1 -1 + , 'p2': 'P2Tree' # 204 435 105 90 -1 -1 + , 'quit': 'finished' # 452 61 -1 -1 -1 -1 + , 'office': 'OfficeTree' # 204 508 75 90 -1 -1 + }, + autonomy={'nav': Autonomy.High, + 'p1': Autonomy.Full, + 'p2': Autonomy.Full, + 'quit': Autonomy.Full, + 'office': Autonomy.Full}) + + # x:474 y:169 + OperatableStateMachine.add('LogFailure', + LogKeyState(text="Failure {}", + severity=2), + transitions={'done': 'ChooseTree' # 272 162 -1 -1 149 90 + }, + autonomy={'done': Autonomy.Off}, + remapping={'data': 'msg'}) + + # x:685 y:248 + OperatableStateMachine.add('LogSuccess', + LogKeyState(text="{}", + severity=2), + transitions={'done': 'ChooseTree' # 566 136 713 247 -1 -1 + }, + autonomy={'done': Autonomy.Off}, + remapping={'data': 'msg'}) + + # x:219 y:227 + OperatableStateMachine.add('NavTree', + RunBtCppState(action_topic='/flexbe_bt_server', + timeout=2.0), + transitions={'success': 'LogSuccess' # 589 244 -1 -1 684 260 + , 'failure': 'LogFailure' # 436 208 -1 -1 473 199 + , 'invalid': 'LogFailure' # 436 208 -1 -1 473 199 + }, + autonomy={'success': Autonomy.Low, + 'failure': Autonomy.Off, + 'invalid': Autonomy.Off}, + remapping={'bt_name': 'nav_tree', + 'bt_payload': 'bt_payload', + 'msg': 'msg'}) + + # x:603 y:515 + OperatableStateMachine.add('OfficeTree', + RunBtCppState(action_topic='/flexbe_bt_server', + timeout=2.0), + transitions={'success': 'LogSuccess' # 733 422 686 514 724 301 + , 'failure': 'LogFailure' # 590 378 638 514 541 222 + , 'invalid': 'LogFailure' # 590 378 638 514 541 222 + }, + autonomy={'success': Autonomy.Low, + 'failure': Autonomy.Off, + 'invalid': Autonomy.Off}, + remapping={'bt_name': 'office_tree', + 'bt_payload': 'bt_payload', + 'msg': 'msg'}) + + # x:226 y:319 + OperatableStateMachine.add('P1Tree', + RunBtCppState(action_topic='/flexbe_bt_server', + timeout=2.0), + transitions={'success': 'LogSuccess' # 597 301 -1 -1 -1 -1 + , 'failure': 'LogFailure' # 431 292 -1 -1 481 222 + , 'invalid': 'LogFailure' # 431 292 -1 -1 481 222 + }, + autonomy={'success': Autonomy.Low, + 'failure': Autonomy.Off, + 'invalid': Autonomy.Off}, + remapping={'bt_name': 'p1_tree', + 'bt_payload': 'bt_payload', + 'msg': 'msg'}) + + # x:402 y:409 + OperatableStateMachine.add('P2Tree', + RunBtCppState(action_topic='/flexbe_bt_server', + timeout=2.0), + transitions={'success': 'LogSuccess' # 655 397 -1 -1 699 301 + , 'failure': 'LogFailure' # 501 344 -1 -1 503 222 + , 'invalid': 'LogFailure' # 501 344 -1 -1 503 222 + }, + autonomy={'success': Autonomy.Low, + 'failure': Autonomy.Off, + 'invalid': Autonomy.Off}, + remapping={'bt_name': 'p2_tree', + 'bt_payload': 'bt_payload', + 'msg': 'msg'}) + + return _state_machine + + # Private functions can be added inside the following tags + # [MANUAL_FUNC] + + + # [/MANUAL_FUNC] diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/delib_ws_p2_sm.py b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/delib_ws_p2_sm.py index 5339aae..b6c5b5a 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/delib_ws_p2_sm.py +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/delib_ws_p2_sm.py @@ -3,17 +3,30 @@ # Copyright 2024 David Conner # -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at +# Redistribution and use in source and binary forms, with or without modification, +# are permitted provided that the following conditions are met: # -# http://www.apache.org/licenses/LICENSE-2.0 +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. + +# 2. 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. +# +# 3. 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. # -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. +# 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. ########################################################### # WARNING: Generated code! # diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/delib_ws_p2_sm_sm.py b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/delib_ws_p2_sm_sm.py index 6c39664..3709a9f 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/delib_ws_p2_sm_sm.py +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/delib_ws_p2_sm_sm.py @@ -3,17 +3,30 @@ # Copyright 2024 David Conner # -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at +# Redistribution and use in source and binary forms, with or without modification, +# are permitted provided that the following conditions are met: # -# http://www.apache.org/licenses/LICENSE-2.0 +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. # -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. +# 2. 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. +# +# 3. 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. ########################################################### # WARNING: Generated code! # diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/detectselect_sm.py b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/detectselect_sm.py index 4a248f4..ad19790 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/detectselect_sm.py +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/detectselect_sm.py @@ -3,17 +3,30 @@ # Copyright 2024 David Conner # -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at +# Redistribution and use in source and binary forms, with or without modification, +# are permitted provided that the following conditions are met: # -# http://www.apache.org/licenses/LICENSE-2.0 +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. + +# 2. 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. +# +# 3. 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. # -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. +# 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. ########################################################### # WARNING: Generated code! # diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/go_beh_sm.py b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/go_beh_sm.py index 312a39d..9562995 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/go_beh_sm.py +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/go_beh_sm.py @@ -1,19 +1,32 @@ #!/usr/bin/env python # -*- coding: utf-8 -*- -# Copyright 2024 Conner +# Copyright 2024 David Conner # -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at +# Redistribution and use in source and binary forms, with or without modification, +# are permitted provided that the following conditions are met: # -# http://www.apache.org/licenses/LICENSE-2.0 +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. + +# 2. 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. +# +# 3. 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. # -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. +# 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. ########################################################### # WARNING: Generated code! # @@ -28,7 +41,7 @@ Go to target behavior Created on Sun Sep 08 2024 -@author: Conner +@author: David Conner """ diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/patrol_sm.py b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/patrol_sm.py index 505a49f..4e186e9 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/patrol_sm.py +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/patrol_sm.py @@ -1,19 +1,32 @@ #!/usr/bin/env python # -*- coding: utf-8 -*- -# Copyright 2024 Conner +# Copyright 2024 David Conner # -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at +# Redistribution and use in source and binary forms, with or without modification, +# are permitted provided that the following conditions are met: # -# http://www.apache.org/licenses/LICENSE-2.0 +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. + +# 2. 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. +# +# 3. 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. # -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. +# 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. ########################################################### # WARNING: Generated code! # @@ -28,7 +41,7 @@ Patrol rooms without concern for battery Created on Mon Sep 23 2024 -@author: Conner +@author: David Conner """ diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/patrolcharge_sm.py b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/patrolcharge_sm.py index f2efc1a..9bc53d9 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/patrolcharge_sm.py +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/patrolcharge_sm.py @@ -1,19 +1,32 @@ #!/usr/bin/env python # -*- coding: utf-8 -*- -# Copyright 2024 Conner +# Copyright 2024 David Conner # -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at +# Redistribution and use in source and binary forms, with or without modification, +# are permitted provided that the following conditions are met: # -# http://www.apache.org/licenses/LICENSE-2.0 +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. + +# 2. 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. +# +# 3. 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. # -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. +# 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. ########################################################### # WARNING: Generated code! # @@ -28,7 +41,7 @@ Patrol and recharge as necessary Created on Mon Sep 23 2024 -@author: Conner +@author: David Conner """ diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/test_navigate_sm.py b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/test_navigate_sm.py index 12222e7..d697ea9 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/test_navigate_sm.py +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/test_navigate_sm.py @@ -3,17 +3,30 @@ # Copyright 2024 David Conner # -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at +# Redistribution and use in source and binary forms, with or without modification, +# are permitted provided that the following conditions are met: # -# http://www.apache.org/licenses/LICENSE-2.0 +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. + +# 2. 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. +# +# 3. 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. # -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. +# 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. ########################################################### # WARNING: Generated code! # diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/test_pick_place_sm.py b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/test_pick_place_sm.py index c404561..ed61aac 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/test_pick_place_sm.py +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/test_pick_place_sm.py @@ -3,17 +3,30 @@ # Copyright 2024 David Conner # -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at +# Redistribution and use in source and binary forms, with or without modification, +# are permitted provided that the following conditions are met: # -# http://www.apache.org/licenses/LICENSE-2.0 +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. + +# 2. 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. +# +# 3. 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. # -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. +# 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. ########################################################### # WARNING: Generated code! # diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/test_plan_path_sm.py b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/test_plan_path_sm.py index 46eda68..0824f24 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/test_plan_path_sm.py +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/test_plan_path_sm.py @@ -3,17 +3,30 @@ # Copyright 2024 David Conner # -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at +# Redistribution and use in source and binary forms, with or without modification, +# are permitted provided that the following conditions are met: # -# http://www.apache.org/licenses/LICENSE-2.0 +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. + +# 2. 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. +# +# 3. 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. # -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. +# 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. ########################################################### # WARNING: Generated code! # diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/through_door_sm.py b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/through_door_sm.py index 62f0ad1..bf383e1 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/through_door_sm.py +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/through_door_sm.py @@ -1,19 +1,32 @@ #!/usr/bin/env python # -*- coding: utf-8 -*- -# Copyright 2024 conner +# Copyright 2024 David Conner # -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at +# Redistribution and use in source and binary forms, with or without modification, +# are permitted provided that the following conditions are met: # -# http://www.apache.org/licenses/LICENSE-2.0 +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. + +# 2. 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. +# +# 3. 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. # -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. +# 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. ########################################################### # WARNING: Generated code! # @@ -28,7 +41,7 @@ Go through a door, opening if required Created on Sun Sep 08 2024 -@author: conner +@author: David Conner """ diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/traverse_sm.py b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/traverse_sm.py index 7b9f50f..1346c7d 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/traverse_sm.py +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/pyrobosim_flexbe_behaviors/traverse_sm.py @@ -1,19 +1,32 @@ #!/usr/bin/env python # -*- coding: utf-8 -*- -# Copyright 2024 conner +# Copyright 2024 David Conner # -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at +# Redistribution and use in source and binary forms, with or without modification, +# are permitted provided that the following conditions are met: # -# http://www.apache.org/licenses/LICENSE-2.0 +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. + +# 2. 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. +# +# 3. 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. # -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. +# 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. ########################################################### # WARNING: Generated code! # @@ -28,7 +41,7 @@ Traverse the world opening doors as required Created on Sun Sep 08 2024 -@author: conner +@author: David Conner """ diff --git a/technologies/FlexBE/pyrobosim_flexbe_behaviors/setup.py b/technologies/FlexBE/pyrobosim_flexbe_behaviors/setup.py index b12cb51..84e2e53 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_behaviors/setup.py +++ b/technologies/FlexBE/pyrobosim_flexbe_behaviors/setup.py @@ -17,7 +17,7 @@ maintainer='David Conner', maintainer_email='robotics@cnu.edu', description='Demonstration behaviors for FlexBE using Pyrobosim', - license='Apache 2.0', + license='BSD-3', tests_require=['pytest'], entry_points={ 'console_scripts': [ diff --git a/technologies/FlexBE/pyrobosim_flexbe_btcpp/.clang-format b/technologies/FlexBE/pyrobosim_flexbe_btcpp/.clang-format new file mode 100644 index 0000000..e0f2099 --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_btcpp/.clang-format @@ -0,0 +1,68 @@ +--- +BasedOnStyle: Google +AccessModifierOffset: -2 +ConstructorInitializerIndentWidth: 2 +AlignEscapedNewlinesLeft: false +AlignTrailingComments: true +AllowAllParametersOfDeclarationOnNextLine: false +AllowShortIfStatementsOnASingleLine: false +AllowShortLoopsOnASingleLine: false +AllowShortFunctionsOnASingleLine: None +AlwaysBreakTemplateDeclarations: true +AlwaysBreakBeforeMultilineStrings: false +BreakBeforeBinaryOperators: false +BreakBeforeTernaryOperators: false +BreakConstructorInitializers: BeforeComma +BinPackParameters: true +ColumnLimit: 100 +ConstructorInitializerAllOnOneLineOrOnePerLine: true +DerivePointerBinding: false +PointerBindsToType: true +ExperimentalAutoDetectBinPacking: false +IndentCaseLabels: true +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCSpaceBeforeProtocolList: true +PenaltyBreakBeforeFirstCallParameter: 19 +PenaltyBreakComment: 60 +PenaltyBreakString: 1 +PenaltyBreakFirstLessLess: 1000 +PenaltyExcessCharacter: 1000 +PenaltyReturnTypeOnItsOwnLine: 90 +SpacesBeforeTrailingComments: 2 +Cpp11BracedListStyle: false +Standard: Auto +IndentWidth: 2 +TabWidth: 2 +UseTab: Never +IndentFunctionDeclarationAfterType: false +SpacesInParentheses: false +SpacesInAngles: false +SpaceInEmptyParentheses: false +SpacesInCStyleCastParentheses: false +SpaceAfterControlStatementKeyword: true +SpaceBeforeAssignmentOperators: true +SpaceBeforeParens: Never +ContinuationIndentWidth: 4 +SortIncludes: false +SpaceAfterCStyleCast: false +ReflowComments: false + +# Configure each individual brace in BraceWrapping +BreakBeforeBraces: Custom + +# Control of individual brace wrapping cases +BraceWrapping: { + AfterClass: 'true', + AfterControlStatement: 'true', + AfterEnum : 'true', + AfterFunction : 'true', + AfterNamespace : 'true', + AfterStruct : 'true', + AfterUnion : 'true', + BeforeCatch : 'true', + BeforeElse : 'true', + IndentBraces : 'false', + SplitEmptyFunction: 'false' +} +... diff --git a/technologies/FlexBE/pyrobosim_flexbe_btcpp/CMakeLists.txt b/technologies/FlexBE/pyrobosim_flexbe_btcpp/CMakeLists.txt new file mode 100644 index 0000000..9e3eaf0 --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_btcpp/CMakeLists.txt @@ -0,0 +1,51 @@ +cmake_minimum_required(VERSION 3.8) +project(pyrobosim_flexbe_btcpp) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) + +set(THIS_PACKAGE_DEPS + ament_index_cpp + behaviortree_ros2 + pyrobosim_msgs + pyrobosim_btcpp + rclcpp +) + +# for each dependency, find and include the package +foreach(DEPENDENCY ${THIS_PACKAGE_DEPS}) + find_package(${DEPENDENCY} REQUIRED) +endforeach() + +# create executable +add_executable(flexbe_btcpp_executor src/flexbe_btcpp_executor.cpp) + +ament_target_dependencies(flexbe_btcpp_executor ${THIS_PACKAGE_DEPS}) + +target_include_directories(flexbe_btcpp_executor PUBLIC + $ +) + + +# Install executable and trees +install( + DIRECTORY trees + DESTINATION share/${PROJECT_NAME} +) + +install( + TARGETS flexbe_btcpp_executor + DESTINATION DESTINATION lib/${PROJECT_NAME} +) +install(DIRECTORY + trees + config + launch + DESTINATION share/${PROJECT_NAME}/ + ) + +ament_package() diff --git a/technologies/FlexBE/pyrobosim_flexbe_btcpp/config/pyrobosim_flexbe_btcpp.yaml b/technologies/FlexBE/pyrobosim_flexbe_btcpp/config/pyrobosim_flexbe_btcpp.yaml new file mode 100644 index 0000000..05276ca --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_btcpp/config/pyrobosim_flexbe_btcpp.yaml @@ -0,0 +1,13 @@ +bt_action_server: + ros__parameters: + action_name: "flexbe_bt_server" # Optional (defaults to `bt_action_server`) + tick_frequency: 100 # Optional (defaults to 100 Hz) + groot2_port: 1667 # Optional (defaults to 1667) + ros_plugins_timeout: 1000 # Optional (defaults 1000 ms) + +# plugins: +# - behaviortree_cpp/bt_plugins +# - btcpp_ros2_samples/bt_plugins + + behavior_trees: + - pyrobosim_flexbe_btcpp/trees diff --git a/technologies/FlexBE/pyrobosim_flexbe_btcpp/launch/pyrobosim_flexbe_btcpp.launch.xml b/technologies/FlexBE/pyrobosim_flexbe_btcpp/launch/pyrobosim_flexbe_btcpp.launch.xml new file mode 100644 index 0000000..f6b2939 --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_btcpp/launch/pyrobosim_flexbe_btcpp.launch.xml @@ -0,0 +1,5 @@ + + + + + diff --git a/technologies/FlexBE/pyrobosim_flexbe_btcpp/package.xml b/technologies/FlexBE/pyrobosim_flexbe_btcpp/package.xml new file mode 100644 index 0000000..905b5d1 --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_btcpp/package.xml @@ -0,0 +1,38 @@ + + + + pyrobosim_flexbe_btcpp + 0.0.1 + + BehaviorTree.CPP executor with Action interface. + + Based on pyrobosim_btcpp and portions of + btcpp_ros2_samples code by Davide Faconti. + + + David Conner + David Conner + BSD-3 + + ament_cmake + + rclpy + flexbe_behavior_engine + flexbe_webui + pyrobosim_flexbe_states + + ament_index_cpp + behaviortree_ros2 + pyrobosim_msgs + rclcpp + pyrobosim_btcpp + + ament_lint_auto + ament_lint_common + + + ament_cmake + + + + diff --git a/technologies/FlexBE/pyrobosim_flexbe_btcpp/src/flexbe_btcpp_executor.cpp b/technologies/FlexBE/pyrobosim_flexbe_btcpp/src/flexbe_btcpp_executor.cpp new file mode 100644 index 0000000..84ab333 --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_btcpp/src/flexbe_btcpp_executor.cpp @@ -0,0 +1,283 @@ +// Copyright 2024 David Conner +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// +// 2. 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. +// +// 3. 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. + + +// Based on pyrobosim_btcpp Copyright 2024 Davide Faconti +// and sample_bt_executor.cpp Copyright 2024 Marq Rasmussen +// +// Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated +// documentation files (the "Software"), to deal in the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to +// permit persons to whom the Software is furnished to do so, subject to the following conditions: The above copyright +// notice and this permission notice shall be included in all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE +// WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR +// COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR +// OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + + +#include + +#include + + +// ROS includes +#include +#include + +// BTCPP includes +#include +#include +#include +#include + +// BTCPP nodes used in this package +#include "pyrobosim_btcpp/nodes/open_door_node.hpp" +#include "pyrobosim_btcpp/nodes/close_door_node.hpp" +#include "pyrobosim_btcpp/nodes/detect_object_node.hpp" +#include "pyrobosim_btcpp/nodes/navigate_node.hpp" +#include "pyrobosim_btcpp/nodes/pick_object_node.hpp" +#include "pyrobosim_btcpp/nodes/place_object_node.hpp" + +#include + + +std::filesystem::path GetFilePath(const std::string& filename) +{ + // check first the given path + if(std::filesystem::exists(filename)) + { + return filename; + } + // try appending the package directory + const std::string package_dir = ament_index_cpp::get_package_share_directory("pyrobosim_btcpp"); + const auto package_path = std::filesystem::path(package_dir) / filename; + if(std::filesystem::exists(package_path)) + { + return package_path; + } + throw std::runtime_error("File not found: " + filename); +} + +static const auto kLogger = rclcpp::get_logger("bt_action_server"); + +class FlexibleBTActionServer : public BT::TreeExecutionServer +{ +public: + FlexibleBTActionServer(const rclcpp::NodeOptions& options) : TreeExecutionServer(options) + { + // here we assume that the battery voltage is published as a std_msgs::msg::Float32 + sub_ = node()->create_subscription( + "/robot/robot_state", 10, [this](const pyrobosim_msgs::msg::RobotState::SharedPtr msg) { + // Update the global blackboard + globalBlackboard()->set("battery_level", msg->battery_level); + }); + // Note that the callback above and the execution of the tree accessing the + // global blackboard happen in two different threads. + // The former runs in the MultiThreadedExecutor, while the latter in the thread created + // by TreeExecutionServer. But this is OK because the blackboard is thread-safe. + } + + void onTreeCreated(BT::Tree& tree) override + { + RCLCPP_INFO(kLogger, "onTreeCreated ..."); + logger_cout_ = std::make_shared(tree); + } + + + /** + * @brief onTreeExecutionCompleted is a callback invoked after the tree execution is completed, + * i.e. if it returned SUCCESS/FAILURE or if the action was cancelled by the Action Client. + * + * @param status The status of the tree after the last tick + * @param was_cancelled True if the action was cancelled by the Action Client + * + * @return if not std::nullopt, the string will be sent as [return_message] to the Action Client. + */ + std::optional onTreeExecutionCompleted(BT::NodeStatus status, + bool was_cancelled) override + { + // NOT really needed, even if logger_cout_ may contain a dangling pointer of the tree + // at this point + RCLCPP_INFO(kLogger, "onTreeExecutionCompleted with status=%d (canceled=%d)", int(status), was_cancelled); + logger_cout_.reset(); + return std::string("pyrobosim_flexbe_btcpp tree complete with status=") + std::to_string(int(status)); + } + + /** + * @brief Callback invoked when a goal is received and before the tree is created. + * If it returns false, the goal will be rejected. + */ + bool onGoalReceived(const std::string& tree_name, const std::string& payload) + { + RCLCPP_INFO(kLogger, "onGoalReceived with tree name '%s' with payload '%s'", tree_name.c_str(), payload.c_str()); + return true; + } + + + /** + * @brief registerNodesIntoFactory is a callback invoked after the + * plugins were registered into the BT::BehaviorTreeFactory. + * It can be used to register additional custom nodes manually. + * + * @param factory The factory to use to register nodes + */ + void registerNodesIntoFactory(BT::BehaviorTreeFactory& factory) + { + RCLCPP_INFO(kLogger, "registerNodesIntoFactory - load custom nodes ..."); + // all the actions are done using the same Action Server. + // Therefore single RosNodeParams will do. + BT::RosNodeParams params; + params.nh = this->node(); + params.default_port_value = "execute_action"; + + factory.registerNodeType("CloseDoor", params); + factory.registerNodeType("DetectObject", params); + factory.registerNodeType("Navigate", params); + factory.registerNodeType("OpenDoor", params); + factory.registerNodeType("PickObject", params); + factory.registerNodeType("PlaceObject", params); + RCLCPP_INFO(kLogger, "registerNodesIntoFactory - registered custom nodes."); + } + + /** + * @brief onLoopAfterTick invoked at each loop, after tree.tickOnce(). + * If it returns a valid NodeStatus, the tree will stop and return that status. + * Return std::nullopt to continue the execution. + * + * @param status The status of the tree after the last tick + */ + std::optional onLoopAfterTick(BT::NodeStatus status) + { + //RCLCPP_INFO(kLogger, "pyrobosim_flexbe_btcpp - onLoopAfterTick."); + return std::nullopt; + } + + + /** + * @brief onLoopFeedback is a callback invoked at each loop, after tree.tickOnce(). + * If it returns a valid string, it will be sent as feedback to the Action Client. + * + * If you don't want to return any feedback, return std::nullopt. + */ + std::optional onLoopFeedback() + { + return std::nullopt; + } + + +private: + std::shared_ptr logger_cout_; + rclcpp::Subscription::SharedPtr sub_; +}; + +int main(int argc, char* argv[]) +{ + rclcpp::init(argc, argv); + + rclcpp::NodeOptions options; + auto action_server = std::make_shared(options); + + RCLCPP_INFO(kLogger, "starting pyrobosim_flexbe_btcpp server ..."); + + // TODO: This workaround is for a bug in MultiThreadedExecutor where it can deadlock when spinning without a timeout. + // Deadlock is caused when Publishers or Subscribers are dynamically removed as the node is spinning. + rclcpp::executors::MultiThreadedExecutor exec(rclcpp::ExecutorOptions(), 0, false, + std::chrono::milliseconds(250)); + exec.add_node(action_server->node()); + RCLCPP_INFO(kLogger, "begin spinning the pyrobosim_flexbe_btcpp server ..."); + exec.spin(); + + RCLCPP_INFO(kLogger, "done spinning the pyrobosim_flexbe_btcpp server ..."); + exec.remove_node(action_server->node()); + + rclcpp::shutdown(); +} + +// int main(int argc, char** argv) +// { +// // Create a ROS Node +// rclcpp::init(argc, argv); +// auto nh = std::make_shared("btcpp_executor"); + +// nh->declare_parameter("tree", rclcpp::PARAMETER_STRING); +// nh->declare_parameter("save-model", false); + +// const std::string tree_filename = nh->get_parameter("tree").as_string(); +// const bool save_model = nh->get_parameter("save-model").as_bool(); + +// if(tree_filename.empty()) +// { +// RCLCPP_FATAL(nh->get_logger(), "Missing parameter 'tree' with the path to the Behavior Tree " +// "XML file"); +// return 1; +// } +// const std::filesystem::path filepath = GetFilePath(tree_filename); + +// //---------------------------------- +// // register all the actions in the factory +// BT::BehaviorTreeFactory factory; + +// // all the actions are done using the same Action Server. +// // Therefore single RosNodeParams will do. +// BT::RosNodeParams params; +// params.nh = nh; +// params.default_port_value = "execute_action"; + +// factory.registerNodeType("CloseDoor", params); +// factory.registerNodeType("DetectObject", params); +// factory.registerNodeType("Navigate", params); +// factory.registerNodeType("OpenDoor", params); +// factory.registerNodeType("PickObject", params); +// factory.registerNodeType("PlaceObject", params); + +// // optionally we can display and save the model of the tree +// if(save_model) +// { +// std::string xml_models = BT::writeTreeNodesModelXML(factory); +// std::cout << "--------- Node Models ---------:\n" << xml_models << std::endl; +// std::ofstream of("tree_nodes_model.xml"); +// of << xml_models; +// std::cout << "\nXML model of the tree saved in tree_nodes_model.xml\n" << std::endl; +// } + +// //---------------------------------- +// // load a tree and execute +// BT::Tree tree = factory.createTreeFromFile(filepath.string()); + +// // This will add console messages for each action and condition executed +// BT::StdCoutLogger console_logger(tree); +// console_logger.enableTransitionToIdle(false); + +// // This is the "main loop":xecution is completed once the tick() method returns SUCCESS of FAILURE +// BT::NodeStatus res = tree.tickWhileRunning(); + +// std::cout << "Execution completed. Result: " << BT::toStr(res) << std::endl; + +// return 0; +// } diff --git a/technologies/FlexBE/pyrobosim_flexbe_btcpp/trees/navigation_demo.xml b/technologies/FlexBE/pyrobosim_flexbe_btcpp/trees/navigation_demo.xml new file mode 100644 index 0000000..7d6bfe8 --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_btcpp/trees/navigation_demo.xml @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/technologies/FlexBE/pyrobosim_flexbe_btcpp/trees/office_nav.xml b/technologies/FlexBE/pyrobosim_flexbe_btcpp/trees/office_nav.xml new file mode 100644 index 0000000..0211568 --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_btcpp/trees/office_nav.xml @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/technologies/FlexBE/pyrobosim_flexbe_btcpp/trees/problem1.xml b/technologies/FlexBE/pyrobosim_flexbe_btcpp/trees/problem1.xml new file mode 100644 index 0000000..0f74212 --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_btcpp/trees/problem1.xml @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/technologies/FlexBE/pyrobosim_flexbe_btcpp/trees/problem2.xml b/technologies/FlexBE/pyrobosim_flexbe_btcpp/trees/problem2.xml new file mode 100644 index 0000000..d414143 --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_btcpp/trees/problem2.xml @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/technologies/FlexBE/pyrobosim_flexbe_states/package.xml b/technologies/FlexBE/pyrobosim_flexbe_states/package.xml index 63f698f..75dfdd5 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_states/package.xml +++ b/technologies/FlexBE/pyrobosim_flexbe_states/package.xml @@ -14,7 +14,7 @@ David Conner David Conner - Apache 2 + BSD-3 rclpy flexbe_core diff --git a/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/check_door_state.py b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/check_door_state.py index a2712cd..81ea6b3 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/check_door_state.py +++ b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/check_door_state.py @@ -2,17 +2,30 @@ # Copyright 2024 Christopher Newport University # -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at +# Redistribution and use in source and binary forms, with or without modification, +# are permitted provided that the following conditions are met: # -# http://www.apache.org/licenses/LICENSE-2.0 +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. # -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. +# 2. 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. +# +# 3. 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. """Check pyrobosim door FlexBE state.""" diff --git a/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/detect_local_objects_state.py b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/detect_local_objects_state.py index 3a69644..c917c59 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/detect_local_objects_state.py +++ b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/detect_local_objects_state.py @@ -2,17 +2,30 @@ # Copyright 2024 Christopher Newport University # -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at +# Redistribution and use in source and binary forms, with or without modification, +# are permitted provided that the following conditions are met: # -# http://www.apache.org/licenses/LICENSE-2.0 +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. # -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. +# 2. 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. +# +# 3. 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. """FlexBE State to request local objects for PyRoboSim robot.""" diff --git a/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/detect_objects_state.py b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/detect_objects_state.py index babec03..494177e 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/detect_objects_state.py +++ b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/detect_objects_state.py @@ -2,17 +2,30 @@ # Copyright 2024 Christopher Newport University # -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at +# Redistribution and use in source and binary forms, with or without modification, +# are permitted provided that the following conditions are met: # -# http://www.apache.org/licenses/LICENSE-2.0 +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. # -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. +# 2. 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. +# +# 3. 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. """FlexBE State to command PyRoboSim robot to detect objects at the current location.""" diff --git a/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/door_action_state.py b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/door_action_state.py index 29db4f6..aafc771 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/door_action_state.py +++ b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/door_action_state.py @@ -2,17 +2,30 @@ # Copyright 2024 Christopher Newport University # -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at +# Redistribution and use in source and binary forms, with or without modification, +# are permitted provided that the following conditions are met: # -# http://www.apache.org/licenses/LICENSE-2.0 +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. # -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. +# 2. 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. +# +# 3. 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. """FlexBE State to command PyRoboSim robot to open or close a door at current location.""" diff --git a/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/follow_path_state.py b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/follow_path_state.py index e92d850..b87b8fc 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/follow_path_state.py +++ b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/follow_path_state.py @@ -2,17 +2,30 @@ # Copyright 2024 Christopher Newport University # -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at +# Redistribution and use in source and binary forms, with or without modification, +# are permitted provided that the following conditions are met: # -# http://www.apache.org/licenses/LICENSE-2.0 +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. # -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. +# 2. 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. +# +# 3. 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. """FlexBE State to command PyRoboSim robot to plan a path to target location.""" @@ -128,7 +141,7 @@ def on_enter(self, userdata): # Send goal clears prior results self._client.send_goal(self._topic, self._goal, wait_duration=self._server_timeout_sec) except Exception as exc: # pylint: disable=W0703 - # Since a state failure not necessarily causes a behavior failure, + # Since a state failure does not necessarily cause a behavior failure, # it is recommended to only print warnings, not errors. # Using a linebreak before appending the error log enables the operator to collapse details in the GUI. Logger.logwarn(f"Failed to send the '{self}' command:\n {type(exc)} - {exc}") diff --git a/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/monitor_battery_state.py b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/monitor_battery_state.py index 4a1e7d8..de350af 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/monitor_battery_state.py +++ b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/monitor_battery_state.py @@ -2,17 +2,30 @@ # Copyright 2024 Christopher Newport University # -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at +# Redistribution and use in source and binary forms, with or without modification, +# are permitted provided that the following conditions are met: # -# http://www.apache.org/licenses/LICENSE-2.0 +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. # -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. +# 2. 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. +# +# 3. 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. """FlexBE State to monitor PyRoboSim robot battery.""" diff --git a/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/navigate_action_state.py b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/navigate_action_state.py index 4e447bf..9e0560f 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/navigate_action_state.py +++ b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/navigate_action_state.py @@ -2,17 +2,30 @@ # Copyright 2024 Christopher Newport University # -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at +# Redistribution and use in source and binary forms, with or without modification, +# are permitted provided that the following conditions are met: # -# http://www.apache.org/licenses/LICENSE-2.0 +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. # -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. +# 2. 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. +# +# 3. 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. """FlexBE State to navigate PyRoboSim robot to target location.""" diff --git a/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/next_room_state.py b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/next_room_state.py index d4fc543..24e0501 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/next_room_state.py +++ b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/next_room_state.py @@ -2,17 +2,30 @@ # Copyright 2024 Christopher Newport University # -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at +# Redistribution and use in source and binary forms, with or without modification, +# are permitted provided that the following conditions are met: # -# http://www.apache.org/licenses/LICENSE-2.0 +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. # -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. +# 2. 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. +# +# 3. 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. """FlexBE State to determine the next room along the way to goal in PyRoboSim world.""" diff --git a/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/pick_action_state.py b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/pick_action_state.py index 2b06ac8..cec1d9f 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/pick_action_state.py +++ b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/pick_action_state.py @@ -2,17 +2,30 @@ # Copyright 2024 Christopher Newport University # -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at +# Redistribution and use in source and binary forms, with or without modification, +# are permitted provided that the following conditions are met: # -# http://www.apache.org/licenses/LICENSE-2.0 +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. # -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. +# 2. 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. +# +# 3. 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. """FlexBE State to command PyRoboSim robot to pick up object at current location.""" diff --git a/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/place_action_state.py b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/place_action_state.py index c376b95..e356c56 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/place_action_state.py +++ b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/place_action_state.py @@ -2,17 +2,30 @@ # Copyright 2024 Christopher Newport University # -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at +# Redistribution and use in source and binary forms, with or without modification, +# are permitted provided that the following conditions are met: # -# http://www.apache.org/licenses/LICENSE-2.0 +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. # -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. +# 2. 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. +# +# 3. 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. """FlexBE State to command PyRoboSim robot to place up object at current location.""" diff --git a/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/plan_path_state.py b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/plan_path_state.py index 876e543..c272d7a 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/plan_path_state.py +++ b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/plan_path_state.py @@ -2,17 +2,30 @@ # Copyright 2024 Christopher Newport University # -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at +# Redistribution and use in source and binary forms, with or without modification, +# are permitted provided that the following conditions are met: # -# http://www.apache.org/licenses/LICENSE-2.0 +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. # -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. +# 2. 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. +# +# 3. 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. """FlexBE State to command PyRoboSim robot to plan a path to target location.""" @@ -146,7 +159,7 @@ def on_enter(self, userdata): self._client.send_goal(self._topic, self._goal, wait_duration=self._planning_timeout.nanoseconds * 1e-9) self._start_time = self._node.get_clock().now() except Exception as exc: # pylint: disable=W0703 - # Since a state failure not necessarily causes a behavior failure, + # Since a state failure does not necessarily cause a behavior failure, # it is recommended to only print warnings, not errors. # Using a linebreak before appending the error log enables the operator to collapse details in the GUI. Logger.logwarn(f"Failed to send the '{self}' command:\n {type(exc)} - {exc}") diff --git a/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/run_btcpp_tree_state.py b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/run_btcpp_tree_state.py new file mode 100644 index 0000000..610a50b --- /dev/null +++ b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/run_btcpp_tree_state.py @@ -0,0 +1,205 @@ +#!/usr/bin/env python + +# Copyright 2024 Christopher Newport University +# +# Redistribution and use in source and binary forms, with or without modification, +# are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. 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. +# +# 3. 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. + +"""FlexBE State to command PyRoboSim robot to plan a path to target location.""" + +from action_msgs.msg import GoalStatus + +from flexbe_core import EventState, Logger +from flexbe_core.proxy import ProxyActionClient + +from geometry_msgs.msg import Pose + +from btcpp_ros2_interfaces.action import ExecuteTree +from btcpp_ros2_interfaces.msg import NodeStatus + +from rclpy.duration import Duration + + +class RunBtCppState(EventState): + """ + FlexBE state to request execution of Behavior Tree using BT.cpp system. + + Elements defined here for UI + Parameters + -- action_topic Action topic name (default= '/flexbe_bt_server') + -- timeout Total time to wait for bt server in seconds (default = 2s) + + Outputs + <= success Successfully executed tree + <= failure Tree failure + <= invalid Invalid tree specified + + User data + ># bt_name string Tree name + ># bt_payload string Payload to pass to BT executor + #> msg string Result message + """ + + def __init__(self, action_topic='/flexbe_bt_server', + timeout=2.0): + # See example_state.py for basic explanations. + super().__init__(outcomes=['success', 'failure', 'invalid'], + input_keys=['bt_name', 'bt_payload'], + output_keys=['msg']) + + self._topic = action_topic + self._server_timeout = Duration(seconds=timeout) + + self._client = ProxyActionClient({self._topic: ExecuteTree}, + wait_duration=0.0) # no need to wait here, we'll check on_enter + + self._return = None # Retain return value in case the outcome is blocked by operator + + def execute(self, userdata): + """ + Call periodically while this state is active. + + Check if the action has been finished and evaluate the result. + """ + if self._return is not None: + # Return prior outcome in case transition is blocked by autonomy level + return self._return + + status = self._client.get_status(self._topic) # get status before clearing result + if self._client.has_result(self._topic): + # Check if the action has been finished + result = self._client.get_result(self._topic, clear=True) + Logger.localinfo(f"action result={str(result)}") + userdata.msg = result.return_message # Output message + if status == GoalStatus.STATUS_SUCCEEDED: + node_status = result.node_status.status + if node_status == NodeStatus.SUCCESS: + Logger.localinfo(f"'{self}' - behavior tree returned success!") + self._return = 'success' + else: + Logger.localwarn(f"'{self}' : '{self._topic}' - behavior tree " + f"returned ({node_status}) '{result.return_message}'") + self._return = 'failure' + return self._return + else: + Logger.logwarn(f"{self} : '{self._topic}' - invalid action status '{result.return_message}'") + self._return = 'failure' + + # Otherwise check for action status change + if status == GoalStatus.STATUS_CANCELED: + Logger.loginfo(f" '{self}' : '{self._topic}' - behavior tree was canceled! ") + self._return = 'failure' + elif status == GoalStatus.STATUS_ABORTED: + Logger.loginfo(f" '{self}' : '{self._topic}' - behavior tree was aborted! ") + self._return = 'failure' + + # If the action has not yet finished, None outcome will be returned and the state stays active. + return self._return + + def on_enter(self, userdata): + """Call when state becomes active.""" + # make sure to reset the data from prior executions + Logger.localinfo(f"on_enter '{self}' - '{self.path}' ...") + self._return = None + userdata.msg = '' + self._client.remove_result(self._topic) # clear any prior result from action server + if 'bt_name' not in userdata: + self._return = 'invalid' + userdata.msg = f"RunBtCppState '{self}' requires userdata.bt_name key!" + Logger.localwarn(userdata.msg) + return + + bt_name = userdata.bt_name + bt_payload = '' + if bt_payload in userdata: + bt_payload = userdata.bt_payload + + # Send the goal. + try: + goal = ExecuteTree.Goal(target_tree=bt_name, payload=bt_payload) + self._client.send_goal(self._topic, goal, wait_duration=self._server_timeout.nanoseconds * 1e-9) + except Exception as exc: # pylint: disable=W0703 + # Since a state failure does not necessarily cause a behavior failure, + # it is recommended to only print warnings, not errors. + # Using a linebreak before appending the error log enables the operator to collapse details in the GUI. + Logger.logwarn(f"Failed to send the '{self}' command:\n {type(exc)} - {exc}") + self._return = 'invalid' + + def on_exit(self, userdata): + """Call when state is deactivated.""" + # Make sure that the action is not running when leaving this state. + # A situation where the action would still be active is for example when the operator manually triggers an outcome. + + if self._client.is_active(self._topic): + self._client.cancel(self._topic) + + # Check for action status change (blocking call!) + is_terminal, status = self._client.verify_action_status(self._topic, 0.1) + if status == GoalStatus.STATUS_CANCELED: + Logger.loginfo(f" '{self}' : '{self._topic}' - request to run BT was canceled! ") + self._return = 'failure' + else: + status_string = self._client.get_status_string(self._topic) + Logger.loginfo(f" '{self}' : '{self._topic}' - Requested to cancel an active BT '{status_string}'" + f" (terminal={is_terminal})") + self._return = 'failure' + + # Local message are shown in terminal but not the UI + if self._return == 'done': + Logger.localinfo('Successfully executed the BT.') + else: + Logger.localwarn('failure to execute the BT.') + Logger.localinfo(f"on_exit '{self}' - '{self.path}' ...") + + def on_pause(self): + """Execute each time this state is paused.""" + Logger.localinfo(f"on_pause '{self}' - '{self.path}' ...") + if self._client.is_active(self._topic): + self._client.cancel(self._topic) + # Check for action status change (blocking call!) + is_terminal, status = self._client.verify_action_status(self._topic, 0.1) + if status == GoalStatus.STATUS_CANCELED: + Logger.loginfo(f" '{self}' : '{self._topic}' - request to run BT was canceled! ") + self._return = 'failure' + else: + status_string = self._client.get_status_string(self._topic) + Logger.loginfo(f" '{self}' : '{self._topic}' - Requested to cancel an active BT '{status_string}'" + f" (terminal={is_terminal})") + self._return = 'failure' + + def on_resume(self, userdata): + """Execute each time this state is resumed.""" + Logger.localinfo(f"on_resume '{self}' - '{self.path}' ...") + if self._return is None: + Logger.localinfo(f"Cannot resume BT '{self}' - require new request ...") + self._return = 'failure' + + def on_start(self): + """Call when behavior starts.""" + Logger.localinfo(f" on_start '{self}' - '{self.path}' ") + + def on_stop(self): + """Call when behavior stops.""" + Logger.localinfo(f" on_stop '{self}' - '{self.path}'") diff --git a/technologies/FlexBE/pyrobosim_flexbe_states/setup.py b/technologies/FlexBE/pyrobosim_flexbe_states/setup.py index 6f93354..a4b388c 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_states/setup.py +++ b/technologies/FlexBE/pyrobosim_flexbe_states/setup.py @@ -23,7 +23,7 @@ maintainer='David Conner', maintainer_email='robotics@cnu.edu', description='Interface FlexBE state implementations for Pyrobosim', - license='Apache 2.0', + license='BSD-3', tests_require=['pytest'], entry_points={ 'console_scripts': [ diff --git a/technologies/FlexBE/pyrobosim_flexbe_utilities/package.xml b/technologies/FlexBE/pyrobosim_flexbe_utilities/package.xml index d1fd175..ebce278 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_utilities/package.xml +++ b/technologies/FlexBE/pyrobosim_flexbe_utilities/package.xml @@ -13,7 +13,7 @@ David Conner David Conner - Apache 2 + BSD-3 rclpy flexbe_core diff --git a/technologies/FlexBE/pyrobosim_flexbe_utilities/pyrobosim_flexbe_utilities/world_configuration.py b/technologies/FlexBE/pyrobosim_flexbe_utilities/pyrobosim_flexbe_utilities/world_configuration.py index 54cf1f8..bf1a651 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_utilities/pyrobosim_flexbe_utilities/world_configuration.py +++ b/technologies/FlexBE/pyrobosim_flexbe_utilities/pyrobosim_flexbe_utilities/world_configuration.py @@ -2,17 +2,30 @@ # Copyright 2024 Christopher Newport University # -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at +# Redistribution and use in source and binary forms, with or without modification, +# are permitted provided that the following conditions are met: # -# http://www.apache.org/licenses/LICENSE-2.0 +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. # -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. +# 2. 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. +# +# 3. 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. """FlexBE utility for loading PyRoboSim world structure.""" diff --git a/technologies/FlexBE/pyrobosim_flexbe_utilities/setup.py b/technologies/FlexBE/pyrobosim_flexbe_utilities/setup.py index b947c90..2d01642 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_utilities/setup.py +++ b/technologies/FlexBE/pyrobosim_flexbe_utilities/setup.py @@ -21,7 +21,7 @@ maintainer='David Conner', maintainer_email='robotics@cnu.edu', description='Pyrobosim utilities used by pyrobosim_flexbe_states ', - license='Apache 2.0', + license='BSD-3', tests_require=['pytest'], entry_points={ 'console_scripts': [ From d36cd8693fa663e93c1451e7dad5fc9e94542157 Mon Sep 17 00:00:00 2001 From: David Conner Date: Wed, 2 Oct 2024 22:21:13 -0400 Subject: [PATCH 48/51] add tic_count_; fix on_exit message; clean up --- technologies/FlexBE/README.md | 2 - .../src/flexbe_btcpp_executor.cpp | 78 +++---------------- .../run_btcpp_tree_state.py | 6 +- 3 files changed, 13 insertions(+), 73 deletions(-) diff --git a/technologies/FlexBE/README.md b/technologies/FlexBE/README.md index 3c1c09b..cbc4c8d 100644 --- a/technologies/FlexBE/README.md +++ b/technologies/FlexBE/README.md @@ -138,7 +138,5 @@ Please use the following publications for reference when using FlexBE and the Fl - Joshua Zutell, David C. Conner, and Philipp Schillinger, ["ROS 2-Based Flexible Behavior Engine for Flexible Navigation"](http://dx.doi.org/10.1109/SoutheastCon48659.2022.9764047), IEEE SouthEastCon, April 2022. -- Joshua Zutell, David C. Conner, and Philipp Schillinger, ["Flexible Behavior Trees: In search of the mythical HFSMBTH for Collaborative Autonomy in Robotics"](https://doi.org/10.48550/arXiv.2203.05389), 2022. - - Samuel Raymond, Grace Walters, Joshua Luzier, and David C. Conner, "Design and Development of the FlexBE WebUI with Introductory Tutorials", J. Comput. Sci. Coll vol.40, no.3, CCSC Eastern, to appear October 2024. diff --git a/technologies/FlexBE/pyrobosim_flexbe_btcpp/src/flexbe_btcpp_executor.cpp b/technologies/FlexBE/pyrobosim_flexbe_btcpp/src/flexbe_btcpp_executor.cpp index 84ab333..953f655 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_btcpp/src/flexbe_btcpp_executor.cpp +++ b/technologies/FlexBE/pyrobosim_flexbe_btcpp/src/flexbe_btcpp_executor.cpp @@ -107,6 +107,7 @@ class FlexibleBTActionServer : public BT::TreeExecutionServer { RCLCPP_INFO(kLogger, "onTreeCreated ..."); logger_cout_ = std::make_shared(tree); + tic_count_ = 0; } @@ -122,11 +123,13 @@ class FlexibleBTActionServer : public BT::TreeExecutionServer std::optional onTreeExecutionCompleted(BT::NodeStatus status, bool was_cancelled) override { - // NOT really needed, even if logger_cout_ may contain a dangling pointer of the tree - // at this point - RCLCPP_INFO(kLogger, "onTreeExecutionCompleted with status=%d (canceled=%d)", int(status), was_cancelled); + RCLCPP_INFO(kLogger, "onTreeExecutionCompleted with status=%d (canceled=%d) after %d tics", + int(status), was_cancelled, tic_count_); logger_cout_.reset(); - return std::string("pyrobosim_flexbe_btcpp tree complete with status=") + std::to_string(int(status)); + std::string result = "pyrobosim_flexbe_btcpp tree completed with status=" + std::to_string(int(status)) + + " after " + std::to_string(tic_count_) + " tics"; + + return result; } /** @@ -172,9 +175,10 @@ class FlexibleBTActionServer : public BT::TreeExecutionServer * * @param status The status of the tree after the last tick */ - std::optional onLoopAfterTick(BT::NodeStatus status) + std::optional onLoopAfterTick(BT::NodeStatus /*status*/) { //RCLCPP_INFO(kLogger, "pyrobosim_flexbe_btcpp - onLoopAfterTick."); + ++tic_count_; return std::nullopt; } @@ -194,6 +198,7 @@ class FlexibleBTActionServer : public BT::TreeExecutionServer private: std::shared_ptr logger_cout_; rclcpp::Subscription::SharedPtr sub_; + uint32_t tic_count_; }; int main(int argc, char* argv[]) @@ -218,66 +223,3 @@ int main(int argc, char* argv[]) rclcpp::shutdown(); } - -// int main(int argc, char** argv) -// { -// // Create a ROS Node -// rclcpp::init(argc, argv); -// auto nh = std::make_shared("btcpp_executor"); - -// nh->declare_parameter("tree", rclcpp::PARAMETER_STRING); -// nh->declare_parameter("save-model", false); - -// const std::string tree_filename = nh->get_parameter("tree").as_string(); -// const bool save_model = nh->get_parameter("save-model").as_bool(); - -// if(tree_filename.empty()) -// { -// RCLCPP_FATAL(nh->get_logger(), "Missing parameter 'tree' with the path to the Behavior Tree " -// "XML file"); -// return 1; -// } -// const std::filesystem::path filepath = GetFilePath(tree_filename); - -// //---------------------------------- -// // register all the actions in the factory -// BT::BehaviorTreeFactory factory; - -// // all the actions are done using the same Action Server. -// // Therefore single RosNodeParams will do. -// BT::RosNodeParams params; -// params.nh = nh; -// params.default_port_value = "execute_action"; - -// factory.registerNodeType("CloseDoor", params); -// factory.registerNodeType("DetectObject", params); -// factory.registerNodeType("Navigate", params); -// factory.registerNodeType("OpenDoor", params); -// factory.registerNodeType("PickObject", params); -// factory.registerNodeType("PlaceObject", params); - -// // optionally we can display and save the model of the tree -// if(save_model) -// { -// std::string xml_models = BT::writeTreeNodesModelXML(factory); -// std::cout << "--------- Node Models ---------:\n" << xml_models << std::endl; -// std::ofstream of("tree_nodes_model.xml"); -// of << xml_models; -// std::cout << "\nXML model of the tree saved in tree_nodes_model.xml\n" << std::endl; -// } - -// //---------------------------------- -// // load a tree and execute -// BT::Tree tree = factory.createTreeFromFile(filepath.string()); - -// // This will add console messages for each action and condition executed -// BT::StdCoutLogger console_logger(tree); -// console_logger.enableTransitionToIdle(false); - -// // This is the "main loop":xecution is completed once the tick() method returns SUCCESS of FAILURE -// BT::NodeStatus res = tree.tickWhileRunning(); - -// std::cout << "Execution completed. Result: " << BT::toStr(res) << std::endl; - -// return 0; -// } diff --git a/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/run_btcpp_tree_state.py b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/run_btcpp_tree_state.py index 610a50b..e57839c 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/run_btcpp_tree_state.py +++ b/technologies/FlexBE/pyrobosim_flexbe_states/pyrobosim_flexbe_states/run_btcpp_tree_state.py @@ -91,12 +91,12 @@ def execute(self, userdata): if self._client.has_result(self._topic): # Check if the action has been finished result = self._client.get_result(self._topic, clear=True) - Logger.localinfo(f"action result={str(result)}") userdata.msg = result.return_message # Output message if status == GoalStatus.STATUS_SUCCEEDED: node_status = result.node_status.status if node_status == NodeStatus.SUCCESS: - Logger.localinfo(f"'{self}' - behavior tree returned success!") + Logger.localinfo(f"'{self}' - behavior tree returned success!" + f" '{result.return_message}'") self._return = 'success' else: Logger.localwarn(f"'{self}' : '{self._topic}' - behavior tree " @@ -167,7 +167,7 @@ def on_exit(self, userdata): self._return = 'failure' # Local message are shown in terminal but not the UI - if self._return == 'done': + if self._return == 'success': Logger.localinfo('Successfully executed the BT.') else: Logger.localwarn('failure to execute the BT.') From 5b4ded8c9ebcb97d5428c90a19b81bf464cbaa3d Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Thu, 3 Oct 2024 17:28:23 +0200 Subject: [PATCH 49/51] adding more actions to BTCPP executor (#40) * adding more actionsto BTCPP executor * fixes and more documentation * more notes --- technologies/BehaviorTree.CPP/README.md | 31 +++++++++++++++ .../pyrobosim_btcpp/nodes/close_door_node.hpp | 0 .../nodes/close_fridge_node.hpp | 0 .../pyrobosim_btcpp/nodes/close_node.hpp | 39 +++++++++++++++++++ .../nodes/detect_object_node.hpp | 38 ++++++++++++++++++ .../nodes/execute_task_node.hpp | 4 ++ .../pyrobosim_btcpp/nodes/navigate_node.hpp | 2 + .../pyrobosim_btcpp/nodes/open_door_node.hpp | 0 .../nodes/open_fridge_node.hpp | 0 .../pyrobosim_btcpp/nodes/open_node.hpp | 39 +++++++++++++++++++ .../nodes/pick_object_node.hpp | 38 ++++++++++++++++++ .../nodes/place_object_node.hpp | 38 ++++++++++++++++++ .../pyrobosim_btcpp/src/btcpp_executor.cpp | 14 +++++++ .../pyrobosim_btcpp/trees/problem1.xml | 24 ++++++++++++ .../pyrobosim_btcpp/trees/problem2.xml | 18 +++++++++ 15 files changed, 285 insertions(+) delete mode 100644 technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/close_door_node.hpp delete mode 100644 technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/close_fridge_node.hpp create mode 100644 technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/close_node.hpp delete mode 100644 technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/open_door_node.hpp delete mode 100644 technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/open_fridge_node.hpp create mode 100644 technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/open_node.hpp create mode 100644 technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/problem1.xml create mode 100644 technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/problem2.xml diff --git a/technologies/BehaviorTree.CPP/README.md b/technologies/BehaviorTree.CPP/README.md index 5a7c6eb..0aeaf55 100644 --- a/technologies/BehaviorTree.CPP/README.md +++ b/technologies/BehaviorTree.CPP/README.md @@ -2,6 +2,15 @@ This package provides some ready to use Nodes (i.e. "actions") to be used with **pyrobosim**. + +We suggest creating your XML trees in the folder `technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees` + +## How to compile + +``` +colcon build --symlink-install --packages-select pyrobosim_btcpp +``` + ## How to run A BTCPP executor is provided to run Behavior Trees stored as XML files @@ -29,3 +38,25 @@ You can run the BeahviorTree with the command: ``` ros2 run pyrobosim_btcpp btcpp_executor --ros-args -p tree:=trees/navigation_demo.xml ``` + +The argument `tree` above is the path to the XML file. The path can be either: + + - absolute, + - relative to the folder where the command is executed + - relative to the package folder, i.e. `technologies/BehaviorTree.CPP/pyrobosim_btcpp`. + +## Implemented Action Nodes + +| Action Name | Description | Input Port | +|--------------|---------------------------------------------|--------------------------------------------------------------------------------| +| Close | Close a door or a container | - location: name of the location | +| DetectObject | Return SUCCESS if the object is detected | - object: name of the object | +| Navigate | Move to a specific location | - target: name of the location | +| Open | Open a door or a container | - object: name of the object | +| PickObject | Pick an object (you must be in front of it) | - object: name of the object
- location: (optional) where the object is | +| PlaceObject | Place an object (you must hold it) | - object: name of the object
- location: (optional) where the object should go | + +Note: + +- the name of door bewtween two rooms is called "hall_room1_room2". For instance "hall_office_kitchen" or "hall_dining_trash". +- names of openable/closable objects are "dumpster", "pantry", "desk" and "fridge". diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/close_door_node.hpp b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/close_door_node.hpp deleted file mode 100644 index e69de29..0000000 diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/close_fridge_node.hpp b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/close_fridge_node.hpp deleted file mode 100644 index e69de29..0000000 diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/close_node.hpp b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/close_node.hpp new file mode 100644 index 0000000..d6100cb --- /dev/null +++ b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/close_node.hpp @@ -0,0 +1,39 @@ +#pragma once + +#include +#include +#include "execute_task_node.hpp" + +namespace BT +{ + +class CloseAction : public ExecuteTaskNode +{ +public: + CloseAction(const std::string& name, const NodeConfig& conf, const RosNodeParams& params) + : ExecuteTaskNode(name, conf, params) + {} + + // specify the ports offered by this node + static BT::PortsList providedPorts() + { + return ExecuteTaskNode::appendProvidedPorts({ BT::InputPort("target") }); + } + + // Implement the method that sends the goal + bool setGoal(TaskAction& action) override + { + std::string target; + if(!getInput("target", target) || target.empty()) + { + throw BT::RuntimeError("missing required input [target]"); + } + + // prepare the goal message + action.type = "close"; + action.target_location = target; + return true; + } +}; + +} // namespace BT diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/detect_object_node.hpp b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/detect_object_node.hpp index e69de29..e4156d5 100644 --- a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/detect_object_node.hpp +++ b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/detect_object_node.hpp @@ -0,0 +1,38 @@ +#pragma once + +#include +#include +#include "execute_task_node.hpp" + +namespace BT +{ + +class DetectObject : public ExecuteTaskNode +{ +public: + DetectObject(const std::string& name, const NodeConfig& conf, const RosNodeParams& params) + : ExecuteTaskNode(name, conf, params) + {} + + // specify the ports offered by this node + static BT::PortsList providedPorts() + { + return ExecuteTaskNode::appendProvidedPorts({ BT::InputPort("object") }); + } + + // Implement the method that sends the goal + bool setGoal(TaskAction& action) override + { + std::string object; + if(!getInput("object", object) || object.empty()) + { + throw BT::RuntimeError("missing required input [object]"); + } + // prepare the goal message + action.type = "detect"; + action.object = object; + return true; + } +}; + +} // namespace BT diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/execute_task_node.hpp b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/execute_task_node.hpp index 562515a..dc549a6 100644 --- a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/execute_task_node.hpp +++ b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/execute_task_node.hpp @@ -1,3 +1,5 @@ +#pragma once + #include #include @@ -80,6 +82,8 @@ inline NodeStatus ExecuteTaskNode::onResultReceived(const ExecutionResult& execu resultToStr(execution_result), execution_result.message.c_str()); return NodeStatus::FAILURE; } + RCLCPP_INFO(logger(), "[%s] succeeded. Message: %s", name().c_str(), + execution_result.message.c_str()); return NodeStatus::SUCCESS; } diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/navigate_node.hpp b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/navigate_node.hpp index 1e0e629..ddbee83 100644 --- a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/navigate_node.hpp +++ b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/navigate_node.hpp @@ -1,3 +1,5 @@ +#pragma once + #include #include #include "execute_task_node.hpp" diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/open_door_node.hpp b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/open_door_node.hpp deleted file mode 100644 index e69de29..0000000 diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/open_fridge_node.hpp b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/open_fridge_node.hpp deleted file mode 100644 index e69de29..0000000 diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/open_node.hpp b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/open_node.hpp new file mode 100644 index 0000000..cba8953 --- /dev/null +++ b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/open_node.hpp @@ -0,0 +1,39 @@ +#pragma once + +#include +#include +#include "execute_task_node.hpp" + +namespace BT +{ + +class OpenAction : public ExecuteTaskNode +{ +public: + OpenAction(const std::string& name, const NodeConfig& conf, const RosNodeParams& params) + : ExecuteTaskNode(name, conf, params) + {} + + // specify the ports offered by this node + static BT::PortsList providedPorts() + { + return ExecuteTaskNode::appendProvidedPorts({ BT::InputPort("target") }); + } + + // Implement the method that sends the goal + bool setGoal(TaskAction& action) override + { + std::string target; + if(!getInput("target", target) || target.empty()) + { + throw BT::RuntimeError("missing required input [target]"); + } + + // prepare the goal message + action.type = "open"; + action.target_location = target; + return true; + } +}; + +} // namespace BT diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/pick_object_node.hpp b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/pick_object_node.hpp index e69de29..f40c32f 100644 --- a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/pick_object_node.hpp +++ b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/pick_object_node.hpp @@ -0,0 +1,38 @@ +#pragma once + +#include +#include +#include "execute_task_node.hpp" + +namespace BT +{ + +class PickObject : public ExecuteTaskNode +{ +public: + PickObject(const std::string& name, const NodeConfig& conf, const RosNodeParams& params) + : ExecuteTaskNode(name, conf, params) + {} + + // specify the ports offered by this node + static BT::PortsList providedPorts() + { + return ExecuteTaskNode::appendProvidedPorts({ BT::InputPort("object") }); + } + + // Implement the method that sends the goal + bool setGoal(TaskAction& action) override + { + std::string object; + if(!getInput("object", object) || object.empty()) + { + throw BT::RuntimeError("missing required input [object]"); + } + // prepare the goal message + action.type = "pick"; + action.object = object; + return true; + } +}; + +} // namespace BT diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/place_object_node.hpp b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/place_object_node.hpp index e69de29..7c9d8fc 100644 --- a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/place_object_node.hpp +++ b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/place_object_node.hpp @@ -0,0 +1,38 @@ +#pragma once + +#include +#include +#include "execute_task_node.hpp" + +namespace BT +{ + +class PlaceObject : public ExecuteTaskNode +{ +public: + PlaceObject(const std::string& name, const NodeConfig& conf, const RosNodeParams& params) + : ExecuteTaskNode(name, conf, params) + {} + + // specify the ports offered by this node + static BT::PortsList providedPorts() + { + return ExecuteTaskNode::appendProvidedPorts({ BT::InputPort("object") }); + } + + // Implement the method that sends the goal + bool setGoal(TaskAction& action) override + { + std::string object; + if(!getInput("object", object) || object.empty()) + { + throw BT::RuntimeError("missing required input [object]"); + } + // prepare the goal message + action.type = "place"; + action.object = object; + return true; + } +}; + +} // namespace BT diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/src/btcpp_executor.cpp b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/src/btcpp_executor.cpp index cfc32d5..ba1d0cc 100644 --- a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/src/btcpp_executor.cpp +++ b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/src/btcpp_executor.cpp @@ -10,7 +10,14 @@ #include // BTCPP nodes in this package +#include "pyrobosim_btcpp/nodes/open_node.hpp" +#include "pyrobosim_btcpp/nodes/close_node.hpp" +#include "pyrobosim_btcpp/nodes/detect_object_node.hpp" #include "pyrobosim_btcpp/nodes/navigate_node.hpp" +#include "pyrobosim_btcpp/nodes/pick_object_node.hpp" +#include "pyrobosim_btcpp/nodes/place_object_node.hpp" + +// TO_WORKSHOP_USER: add here the include to your custom actions, if you have any std::filesystem::path GetFilePath(const std::string& filename) { @@ -59,7 +66,14 @@ int main(int argc, char** argv) params.nh = nh; params.default_port_value = "execute_action"; + factory.registerNodeType("Close", params); + factory.registerNodeType("DetectObject", params); factory.registerNodeType("Navigate", params); + factory.registerNodeType("Open", params); + factory.registerNodeType("PickObject", params); + factory.registerNodeType("PlaceObject", params); + + // TO_WORKSHOP_USER: add here more rgistration, if you decided to implement your own nodes // optionally we can display and save the model of the tree if(save_model) diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/problem1.xml b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/problem1.xml new file mode 100644 index 0000000..b04a77c --- /dev/null +++ b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/problem1.xml @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/problem2.xml b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/problem2.xml new file mode 100644 index 0000000..f12f1a9 --- /dev/null +++ b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/problem2.xml @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + From 02432e0fcc7cf878c91a8fa708df013c759f9090 Mon Sep 17 00:00:00 2001 From: David Conner Date: Thu, 3 Oct 2024 19:44:24 -0400 Subject: [PATCH 50/51] update for latest BT.cpp technology changes --- .../src/flexbe_btcpp_executor.cpp | 10 ++++++---- .../FlexBE/pyrobosim_flexbe_btcpp/trees/problem2.xml | 9 +++++---- 2 files changed, 11 insertions(+), 8 deletions(-) diff --git a/technologies/FlexBE/pyrobosim_flexbe_btcpp/src/flexbe_btcpp_executor.cpp b/technologies/FlexBE/pyrobosim_flexbe_btcpp/src/flexbe_btcpp_executor.cpp index 953f655..ad19b6d 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_btcpp/src/flexbe_btcpp_executor.cpp +++ b/technologies/FlexBE/pyrobosim_flexbe_btcpp/src/flexbe_btcpp_executor.cpp @@ -57,13 +57,15 @@ #include // BTCPP nodes used in this package -#include "pyrobosim_btcpp/nodes/open_door_node.hpp" -#include "pyrobosim_btcpp/nodes/close_door_node.hpp" +#include "pyrobosim_btcpp/nodes/open_node.hpp" +#include "pyrobosim_btcpp/nodes/close_node.hpp" #include "pyrobosim_btcpp/nodes/detect_object_node.hpp" #include "pyrobosim_btcpp/nodes/navigate_node.hpp" #include "pyrobosim_btcpp/nodes/pick_object_node.hpp" #include "pyrobosim_btcpp/nodes/place_object_node.hpp" +// TO_WORKSHOP_USER: add here the include to your custom actions, if you have any + #include @@ -159,10 +161,10 @@ class FlexibleBTActionServer : public BT::TreeExecutionServer params.nh = this->node(); params.default_port_value = "execute_action"; - factory.registerNodeType("CloseDoor", params); + factory.registerNodeType("Close", params); factory.registerNodeType("DetectObject", params); factory.registerNodeType("Navigate", params); - factory.registerNodeType("OpenDoor", params); + factory.registerNodeType("Open", params); factory.registerNodeType("PickObject", params); factory.registerNodeType("PlaceObject", params); RCLCPP_INFO(kLogger, "registerNodesIntoFactory - registered custom nodes."); diff --git a/technologies/FlexBE/pyrobosim_flexbe_btcpp/trees/problem2.xml b/technologies/FlexBE/pyrobosim_flexbe_btcpp/trees/problem2.xml index d414143..571623c 100644 --- a/technologies/FlexBE/pyrobosim_flexbe_btcpp/trees/problem2.xml +++ b/technologies/FlexBE/pyrobosim_flexbe_btcpp/trees/problem2.xml @@ -3,14 +3,15 @@ - + - - + + + + - From 7614ce1d523c9331cd353e77f98dffbf18297ac2 Mon Sep 17 00:00:00 2001 From: David Conner Date: Thu, 3 Oct 2024 21:12:51 -0400 Subject: [PATCH 51/51] add more HFSMBTH info for BT.cpp, including groot2 use --- technologies/FlexBE/docs/hfsmbth.md | 23 +++++++++++++++++++++-- 1 file changed, 21 insertions(+), 2 deletions(-) diff --git a/technologies/FlexBE/docs/hfsmbth.md b/technologies/FlexBE/docs/hfsmbth.md index da91794..29b3506 100644 --- a/technologies/FlexBE/docs/hfsmbth.md +++ b/technologies/FlexBE/docs/hfsmbth.md @@ -13,7 +13,26 @@ Please use the following publication for reference when using the HFSMBTH: * `clear; ros2 launch flexbe_onboard behavior_onboard.launch.py` * `clear; ros2 launch flexbe_webui flexbe_ocs.launch.py headless:=true` * `clear; ros2 run flexbe_webui webui_client` + +Additionally we need to launch the custom `flexbe_btcpp_executor` node which executes the +BT.cpp v4-based behavior trees using an `ExecuteTree` action server interface. * `clear; ros2 launch pyrobosim_flexbe_btcpp pyrobosim_flexbe_btcpp.launch.xml` - And load `BtCppHFSMBTH` behavior. - This uses a `RunBtCppTreeState` and a custom `flexbe_btcpp_executor` node. +This custom executor node loads the node definitions used by this workshop demonstration. + +Via the FlexBE webui load the `BtCppHFSMBTH` behavior and execute. + This uses a `RunBtCppTreeState` to send `ExecuteTree` action goals to the custom `flexbe_btcpp_executor` node. + +We include a subset of the demonstration trees from this workshop in the `pyrobosim_flexbe_btcpp` package; these define unique behavior IDs; e.g. + +```xml + +``` +which is required if they are launched by the `ExecuteTree.action`. + +Additional behavior tree packages may be listed in the `pyrobosim_flexbe_btcpp/config/pyrobosim_flexbe_btcpp.yaml` configuration file, but the required node definitions must be +registered with the `flexbe_btcpp_executor` code. + +The FlexBE operator may preempt the `RunBtCppTreeState` which sends a message to cancel the `ExecuteTree` action to the BT action server, which halts the behavior tree. + +You may optionally start `groot2` to visualize the behavior tree as it executes.