diff --git a/docker-compose.yml b/docker-compose.yml index b7b1817..28122b9 100644 --- a/docker-compose.yml +++ b/docker-compose.yml @@ -53,3 +53,4 @@ services: - SYS_ADMIN devices: - /dev/fuse:/dev/fuse + diff --git a/docs/source/conf.py b/docs/source/conf.py index 7f8cd9d..9402cf6 100644 --- a/docs/source/conf.py +++ b/docs/source/conf.py @@ -19,4 +19,4 @@ html_style = 'css/custom.css' # this allows to reference the images relative to the tutorials folder in md and also sphinx to find them from html -html_extra_path = ['../../tutorials/', '../../tutorials/refine_plan_demo/'] \ No newline at end of file +html_extra_path = ['../../tutorials/', '../../tutorials/refine_plan_demo/', '../../tutorials/semantic_reasoning_demo/'] diff --git a/docs/source/index.rst b/docs/source/index.rst index 60485ed..7dd0aa6 100644 --- a/docs/source/index.rst +++ b/docs/source/index.rst @@ -209,3 +209,4 @@ This is a statistical model checking engine for DTMC models given in JANI, which tutorial_roaml tutorial_refine_plan tutorial_moon + tutorial_semantic_reasoning diff --git a/docs/source/tutorial_semantic_reasoning.rst b/docs/source/tutorial_semantic_reasoning.rst new file mode 100644 index 0000000..4b6a98a --- /dev/null +++ b/docs/source/tutorial_semantic_reasoning.rst @@ -0,0 +1 @@ +.. mdinclude:: ../../tutorials/semantic_reasoning_demo/README.md diff --git a/tutorials/.docker/Dockerfile b/tutorials/.docker/Dockerfile index ce9392f..dfe96b9 100644 --- a/tutorials/.docker/Dockerfile +++ b/tutorials/.docker/Dockerfile @@ -9,7 +9,7 @@ ARG DEBIAN_FRONTEND=noninteractive RUN apt-get update && \ apt-get upgrade -y && \ apt-get install -y \ - apt-utils curl fuse3 libfuse2 gdb nano \ + apt-utils curl fuse3 libfuse2 gdb nano wget \ terminator dbus-x11 \ libegl1 libgl1-mesa-dev libglu1-mesa-dev '^libxcb.*-dev' libx11-xcb-dev \ libxi-dev libxkbcommon-dev libxkbcommon-x11-dev libxrender-dev \ @@ -17,7 +17,7 @@ RUN apt-get update && \ python3-pip python3-tk python3-wrapt python3-inflection \ ros-jazzy-example-interfaces ros-jazzy-rqt-service-caller ros-jazzy-plotjuggler ros-jazzy-plotjuggler-ros \ libczmq-dev ros-jazzy-zmqpp-vendor-dbgsym ros-jazzy-zmqpp-vendor libczmq-dev libczmq4 \ - qt6-scxml* rustup tmux libboost-all-dev libcairo2-dev + qt6-scxml* rustup tmux libboost-all-dev libcairo2-dev nlohmann-json3-dev # Create a ROS 2 workspace. RUN mkdir -p /convince_ws/src/ @@ -26,7 +26,8 @@ WORKDIR /convince_ws # Install external dependencies. # (pip) COPY tutorials/requirements.txt /convince_ws -RUN pip3 install --break-system-packages -r requirements.txt +RUN pip3 install --no-cache-dir --break-system-packages --ignore-installed -r requirements.txt +RUN pip3 install --break-system-packages --ignore-installed "setuptools<70" # (pyrobosim) RUN git clone -b 4.3.4 https://github.com/sea-bass/pyrobosim.git src/pyrobosim @@ -40,11 +41,38 @@ RUN curl -O -L https://pub-32cef6782a9e411e82222dee82af193e.r2.dev/Groot2-v1.9.0 RUN chmod +x Groot2-v1.9.0-x86_64.AppImage && \ mv Groot2-v1.9.0-x86_64.AppImage /usr/local/bin/groot +# (semantic anchoring (ros)) +RUN mkdir /usr/share/desktop-directories +RUN curl -1sLf \ + 'https://repo.typedb.com/public/public-release/setup.deb.sh' \ + | sudo -E bash +RUN apt update &&\ + apt install -y --no-install-recommends typedb=2.28.3 +RUN apt update &&\ + wget -O /tmp/typedb-studio-linux-2.28.6-1.deb https://repo.typedb.com/public/public-release/raw/names/typedb-studio-linux-x86_64-deb/versions/2.28.6/typedb-studio-linux-x86_64-2.28.6.deb && apt install -y --no-install-recommends /tmp/typedb-studio-linux-2.28.6-1.deb && rm /tmp/typedb-studio-linux-2.28.6-1.deb +RUN ln -s /opt/typedb-studio/bin/TypeDB\ Studio /opt/typedb-studio/bin/typedb-studio +RUN mkdir -p /root/.typedb-studio/properties +COPY tutorials/semantic_reasoning_demo/misc/anchoring/tql/typedb-studio.properties /root/.typedb-studio/properties/typedb-studio.properties +RUN mkdir /sit-aw-anchoring +WORKDIR /sit-aw-anchoring +RUN git clone https://github.com/convince-project/sit-aw-anchoring.git /tmp/sit-aw-anchoring-temp +RUN git clone https://github.com/convince-project/sit-aw-skrawl.git /tmp/sit-aw-skrawl-temp +RUN mv /tmp/sit-aw-anchoring-temp/src /sit-aw-anchoring/src && \ + rm -rf /tmp/sit-aw-anchoring-temp +RUN mv /tmp/sit-aw-skrawl-temp/schemas /sit-aw-anchoring/src/anchoring_skrawl_plugins/skrawl/ && \ + rm -rf /tmp/sit-aw-skrawl-temp +RUN rosdep update && \ + rosdep install --from-paths src -y --ignore-src --skip-keys "nlohmann_json" +RUN source /opt/ros/jazzy/setup.bash && \ + colcon build --symlink-install + # (ros) +WORKDIR /convince_ws COPY tutorials/ /convince_ws/src/tutorials/ RUN rosdep update && \ - rosdep install --from-paths src -y --ignore-src + rosdep install --from-paths src -y --ignore-src --skip-keys "anchoring_process anchoring_process_interfaces nlohmann_json" RUN source /opt/ros/jazzy/setup.bash && \ + source /sit-aw-anchoring/install/setup.bash && \ colcon build --symlink-install && \ source install/setup.bash diff --git a/tutorials/.docker/entrypoint.sh b/tutorials/.docker/entrypoint.sh index 239d712..8e217c1 100755 --- a/tutorials/.docker/entrypoint.sh +++ b/tutorials/.docker/entrypoint.sh @@ -2,6 +2,8 @@ source /opt/ros/jazzy/setup.bash +source /sit-aw-anchoring/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" diff --git a/tutorials/semantic_reasoning_demo/README.md b/tutorials/semantic_reasoning_demo/README.md new file mode 100644 index 0000000..84ba2cd --- /dev/null +++ b/tutorials/semantic_reasoning_demo/README.md @@ -0,0 +1,161 @@ +# Semantic Reasoning Tutorial + +This tutorial demonstrates how the semantic reasoning module developed in CONVINCE can identify undesired situations at runtime. + +All commands use `docker compose` and must be executed from the root of the repository, where `docker-compose.yml` is located. + +A video of this tutorial is available on [YouTube](https://youtu.be/xWV7h7M_TRw?si=WKXO4SxqWZpkBg-c). + +## The Problem + +The robot is tasked with navigating a domestic environment to fetch soda and snacks, delivering them to the fridge one at a time. + +![domestic_environment](imgs/pyrobosim.png) + +During the execution of a place action, the force measured at the robot's end effector unexpectedly rises, while the gripper's z-position does not decrease to reach the desired place target. + +This unexpected situation represents an undesired deviation from nominal operational conditions. It can be formally described as a property in temporal logics, allowing monitoring frameworks like [MOON](https://convince-project.github.io/overview/tutorial_moon.html) to detect when it is violated. + +![conceptual_place_property_violation](imgs/srd-conceptual_place_property_violation.png) + +Though, detection alone is insufficient to determine the appropriate intervention. Rising force at the end-effector's tip can be due to multiple causes, such as collisions with objects making the place target inaccessible or adversarial human behavior obstructing the robot arm from reaching the target place. + +Identifying the semantic nature of the anomaly is then crucial to apply informed intervention. + +## Getting Started + +Run this command to bring up all the system's components: + +```bash +docker compose run --rm base ros2 launch semantic_reasoning_bringup bringup.launch.py +``` + +System's components include: + +* `/demo`. An instance of PyRoboSim providing the virtual domestic environment for the robot to navigate. +* `/plotjuggler`. An instance of PlotJuggler that displays the values of a simple virtual force sensor at the tip of the robot's end-effector. +* `/tutorial_skill_executor`. A simple skill execution layer for the mobile robot. Skills are implemented by action servers that use the same interfaces as the [offline verified models](https://convince-project.github.io/overview/tutorial_roaml.html). The actual skill implementation is delegated to PyRoboSim. The component also provides a publisher for the name of the last executed skill and a service server for artificially injecting bugs in the robot's navigation system to force the occurrence of anomalies. +* `/monitor`. An instance of a [MOON](https://convince-project.github.io/overview/tutorial_moon.html) monitor for the place action, configured (see `misc/moon` directory) to verify this property at runtime: when executing `PLACE`, the effort shall not exceed 4.5N for more than 200ms. + + ``` + PROPERTY = "( {PLACE} -> ( once[0:0.2]( {effort <= 4.5} ) ) )" + ``` + +* Components related to semantic reasoning (see below). + +![bringup](imgs/srd-bringup.png) + +### Semantic Reasoning Components + +Semantic reasoning components include: + +* `/anchoring_process`. The component that integrates perception data with information in digital twins to produce a symbolic representation of operational environment. The inference mechanism at its core is enabled by a knowledge base constituted by an ontology, SKRAWL, and its rules. `/anchoring_process` is configured with a scene description in JSON format that maps entities in the digital twins to concepts in the SKRAWL ontology (see `misc/anchoring/dt/setup.json`). Information on functional entities comes from `/tutorial_skill_executor`, while that on geometric entities comes from `/tutorial_dt`. +* `/tutorial_dt`. The digital twin that provides geometric information of scene objects to `/anchoring_process`. This includes objects' bounding boxes and and runtime-changing data such as positions and orientations. `/tutorial_dt` uses PyRoboSim's `/request_world_state` service to access geometric information in the virtual scene and then packs it in JSON format for `/anchoring_process` to consume. +* `/monitors_anchoring_interface`. A component that listens to MOON monitors' verdict topics (`/place_monitor/monitor_verdict` in this example) and triggers the execution of the anchoring's knowledge update process when the violation of a monitored property is detected. +* An instance of [TypeDB Studio](https://typedb.com/docs/manual/2.x/studio/), to visually inspect the robot's internal knowledge (SKRAWL). To set up TypeDB Studio, (i) connect to the running TypeDB server (click `Connect to TypeDB` on the top right, then `Connect`); (ii) select the available database (`convince-semantic_reasoning_demo`) from the drop-down menu on the top left; and (iii) click on `Open Project`, then `Open` to access to ready-to-use queries for knowledge inspection. + +## Nominal Execution + +At the beginning, the robot operates without encountering any deviations from the planned mission. It successfully retrieves the soda and transports it to the fridge. Then it navigates seamlessly to the dining's side table and picks up the snacks. + +To execute this initial phase of the mission, where everything proceeds as expected, run: + +```bash +docker compose run --rm base ros2 launch semantic_reasoning_bringup policy_executor.launch.py target_tree:=SRBT1 +``` + +The robot starts moving and logs of the BT status are printed in the console. + +![bt1](imgs/srd-bt1.png) + +The behavior tree of this first part of the mission is `simulation_extras/bt_executor2/behavior_trees/bt_tree_1.xml`. Note, we assume full observability of objects, i.e., the robot does not need to use any object detector to find soda and snacks and add them to the world model. + +## Anomaly Injection + +At this stage, our demo scenario anticipates that the robot drifts during the execution of the next navigation action, causing it to end up at the corner of the fridge rather than in front of it. We simulate this drift by introducing a spurious navigation bug before the navigation action is executed using the following command: + +```bash +docker compose run --rm base ros2 service call /robot/inject/spurious_navigation_bug std_srvs/srv/SetBool "{data: true}" +``` + +Additionally, we assume that the condition check programmed by the developers, which is intended to ensure that the robot has actually reached the target point, is incorrectly implemented. Consequently, the misplacement goes undetected. As a result, when the robot attempts to place the snacks in the fridge, it collides with the fridge's borders, causing the place action to fail. + +This failing second part of the robot's mission can be run with: + +```bash +docker compose run --rm base ros2 launch semantic_reasoning_bringup policy_executor.launch.py target_tree:=SRBT2 +``` + +![bt2](imgs/srd-bt2.png) + +Note how, due to the collision with the fridge's borders, the effort at the robot's end-effector tip rises above the property threshold. MOON's place monitor detects the property violation, and the anchoring's knowledge update process is activated (visible in the console). + +## Robot Knowledge Inspection + +The execution of the anchoring's knowledge update process causes a series of interactions between `/anchoring_process` and the digital twins that are transparent to the user. The properties of ontology individuals are updated and inference rules are applied. The figure below illustrates how the application of inference rules (reasoning) deduces the cause of the non-manifestation of the place affordance for the robot’s place task. All the inferred knowledge is outlined in green. + +![knowledge_inspection_violated_place_precondition](imgs/srd-violated_place_precondition.png) + +`situation` is the central concept that represents the task execution. It serves as the context for a `place_affordance`, which defines a `place` task involving three participants: the `performer` of the action – the `robot`, which possesses the `can_place` capability; the `placeable` entity – the `snacks` held by the `robot`, which has the `placeability` disposition; and the `container` – the `fridge`, which has the `storage` disposition (the target where the `snacks` is to be placed). + +The `situation` acts as the `operational_context` for the non-manifested place affordance. The `cause` of the non-manifestation is the absence of the `is_in_front_of` relation between the `performer` and the `container` (`required_is_in_front_of`), indicating that the `robot` is not in front of the `fridge`. + +The snapshot of the robot's internal knowledge in the above figure is obtained with the following query in TypeDB Studio: + +```bash + match + $nmpa (evaluated_affordance: $pa, operational_context: $sit) isa non_manifested_affordance; + $pa (describes: $cwo, describes: $dwt, describes: $dwr) isa place_affordance; + (has_context: $sit, cause: $cause) isa precondition_violation; + (assigned_role: $ar, player: $p, context: $s) isa role_assignment; + $p isa physical_entity, has name $pn; + get; +``` + +## Mitigation Action + +We assume that the previously described undesired system state corresponds to a known anomaly for which a predefined mitigation action exists. The mitigation strategy involves commanding the robot to move away from the fridge, perform a brief navigational loop in the kitchen, and then return to attempt correct positioning. + +To simulate the successful application of this mitigation strategy, we first remove the injected spurious navigation bug before the robot executes the action to navigate away from the fridge. This is achieved using the following command: + +```bash +docker compose run --rm base ros2 service call /robot/inject/spurious_navigation_bug std_srvs/srv/SetBool "{data: false}" +``` + +The successful final phase of the robot’s mission can then be executed with: + +```bash +docker compose run --rm base ros2 launch semantic_reasoning_bringup policy_executor.launch.py target_tree:=SRBT3 +``` + +In this case, MOON does not activate the anchoring's knowledge update process because the monitored property is not violated. But we can be manually trigger it using the following command: + +```bash +docker compose run --rm base ros2 action send_goal /anchoring_process/update_state anchoring_process_interfaces/action/UpdateState "{knowledge_domain: 'SkrawlMoMa'}" +``` + +After executing the above query, the result will be empty, indicating that there are no longer any non-manifested place affordances in the system: + +```bash +## Result> Get query did not match any concepts in the database. +``` + +Conversely, and as expected, querying the system now confirms that the `robot` is correctly positioned in front of the `fridge`: + +```bash +match + $f (located: $a, location: $p) isa is_in_front_of; + $a isa artificial_agent, has name $an; + $p isa physical_entity, has name $pn; +get; +``` + +![knowledge_inspection_place_precondition_ok](imgs/srd-place_precondition_ok.png) + +## Further Information + +For more information, you can: + +* Visit our official [documentation](https://convince-project.github.io/sit-aw-anchoring/) +* Look at the [source code](https://github.com/convince-project/sit-aw-anchoring) +* Look at the [ontology](https://github.com/convince-project/sit-aw-skrawl) diff --git a/tutorials/semantic_reasoning_demo/anchoring_extra/monitors_anchoring_interface/LICENSE b/tutorials/semantic_reasoning_demo/anchoring_extra/monitors_anchoring_interface/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/tutorials/semantic_reasoning_demo/anchoring_extra/monitors_anchoring_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/tutorials/semantic_reasoning_demo/anchoring_extra/monitors_anchoring_interface/launch/cfg/params.yaml b/tutorials/semantic_reasoning_demo/anchoring_extra/monitors_anchoring_interface/launch/cfg/params.yaml new file mode 100644 index 0000000..51cf1ae --- /dev/null +++ b/tutorials/semantic_reasoning_demo/anchoring_extra/monitors_anchoring_interface/launch/cfg/params.yaml @@ -0,0 +1,5 @@ +monitors_anchoring_interface: + ros__parameters: + verdict_topics: + - "/place_monitor/monitor_verdict" + anchoring_update_state_action: "/anchoring_process/update_state" diff --git a/tutorials/semantic_reasoning_demo/anchoring_extra/monitors_anchoring_interface/launch/monitors_anchoring_interface.launch.py b/tutorials/semantic_reasoning_demo/anchoring_extra/monitors_anchoring_interface/launch/monitors_anchoring_interface.launch.py new file mode 100644 index 0000000..6055efc --- /dev/null +++ b/tutorials/semantic_reasoning_demo/anchoring_extra/monitors_anchoring_interface/launch/monitors_anchoring_interface.launch.py @@ -0,0 +1,27 @@ +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 + +share_dir = get_package_share_directory('monitors_anchoring_interface') + +def generate_launch_description(): + + # Create and declare entities + monitors_anchoring_interface_node = Node( + name='monitors_anchoring_interface', + package='monitors_anchoring_interface', executable='run', + namespace='', + remappings=[ + ], + parameters=[share_dir+'/launch/cfg/params.yaml'], + output='screen', + emulate_tty=True # assure that RCLCPP output gets flushed + ) + + # Launch Description + ld = LaunchDescription() + ld.add_entity(monitors_anchoring_interface_node) + + return ld diff --git a/tutorials/semantic_reasoning_demo/anchoring_extra/monitors_anchoring_interface/monitors_anchoring_interface/__init__.py b/tutorials/semantic_reasoning_demo/anchoring_extra/monitors_anchoring_interface/monitors_anchoring_interface/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/tutorials/semantic_reasoning_demo/anchoring_extra/monitors_anchoring_interface/monitors_anchoring_interface/run.py b/tutorials/semantic_reasoning_demo/anchoring_extra/monitors_anchoring_interface/monitors_anchoring_interface/run.py new file mode 100644 index 0000000..22ff96f --- /dev/null +++ b/tutorials/semantic_reasoning_demo/anchoring_extra/monitors_anchoring_interface/monitors_anchoring_interface/run.py @@ -0,0 +1,149 @@ +import rclpy +from rclpy.node import Node +from rclpy.action import ActionClient +from rclpy.qos import QoSProfile + +from std_msgs.msg import String +from anchoring_process_interfaces.action import UpdateState + + +class MonitorsAnchoringInterface(Node): + + def __init__(self): + super().__init__('monitors_anchoring_interface') + + # --- Declare parameters --- + self.declare_parameter('verdict_topics', rclpy.Parameter.Type.STRING_ARRAY) + self.declare_parameter('anchoring_update_state_action', rclpy.Parameter.Type.STRING) + + # --- Get parameters --- + self.verdict_topics = self.get_parameter('verdict_topics').value + self.action_name = self.get_parameter('anchoring_update_state_action').value + + if not self.verdict_topics: + self.get_logger().error("No verdict_topics provided") + raise RuntimeError("Missing verdict_topics") + + if not self.action_name: + self.get_logger().error("No anchoring_update_state_action provided") + raise RuntimeError("Missing action name") + + # --- QoS --- + qos = QoSProfile(depth=10) + + # --- Action client --- + self.action_client = ActionClient(self, UpdateState, self.action_name) + + # --- State tracking --- + self.previous_states = {} + + # --- Subscriptions --- + self.subscribers = [] + for topic in self.verdict_topics: + sub = self.create_subscription( + String, + topic, + lambda msg, t=topic: self.verdict_callback(msg, t), + qos + ) + self.subscribers.append(sub) + self.previous_states[topic] = None + + self.get_logger().info(f"Subscribed to {topic}") + + # ----------------------------------------- + # Callback + # ----------------------------------------- + def verdict_callback(self, msg: String, topic: str): + + current_value = msg.data.strip().lower() + + if current_value not in ['currently_true', 'currently_false']: + self.get_logger().warn( + f"Unexpected value '{msg.data}' on {topic}") + return + + prev = self.previous_states[topic] + + # --- Detect transition TRUE -> FALSE --- + if prev == 'currently_true' and current_value == 'currently_false': + self.get_logger().info( + f"Transition TRUE->FALSE detected on {topic}") + + # Delay execution instead of immediate call + self._schedule_double_call(topic) + + self.previous_states[topic] = current_value + + # ----------------------------------------- + # Delayed double call + # ----------------------------------------- + def _schedule_double_call(self, topic: str): + + # First call after 0.5s + timer1 = self.create_timer( + 0.5, + lambda t=topic: self._first_call(t, timer1) + ) + + def _first_call(self, topic: str, timer1): + timer1.cancel() + + self.get_logger().info(f"Delayed call 1 for {topic}") + self.send_update_state_goal(topic) + + # Second call after another 0.5s + timer2 = self.create_timer( + 0.5, + lambda t=topic: self._second_call(t, timer2) + ) + + def _second_call(self, topic: str, timer2): + timer2.cancel() + + self.get_logger().info(f"Delayed call 2 for {topic}") + self.send_update_state_goal(topic) + + # ----------------------------------------- + # Action handling + # ----------------------------------------- + def send_update_state_goal(self, topic: str): + + if not self.action_client.wait_for_server(timeout_sec=2.0): + self.get_logger().error( + f"Action server '{self.action_name}' not available") + return + + goal_msg = UpdateState.Goal() + goal_msg.knowledge_domain = 'SkrawlMoMa' + + future = self.action_client.send_goal_async(goal_msg) + future.add_done_callback(self._goal_response_callback) + + def _goal_response_callback(self, future): + goal_handle = future.result() + + if not goal_handle.accepted: + self.get_logger().warn("Goal rejected") + return + + self.get_logger().info("Goal accepted") + + result_future = goal_handle.get_result_async() + result_future.add_done_callback(self._result_callback) + + def _result_callback(self, future): + result = future.result().result + self.get_logger().info(f"Action result received: {result}") + + +def main(args=None): + rclpy.init(args=args) + node = MonitorsAnchoringInterface() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/tutorials/semantic_reasoning_demo/anchoring_extra/monitors_anchoring_interface/package.xml b/tutorials/semantic_reasoning_demo/anchoring_extra/monitors_anchoring_interface/package.xml new file mode 100644 index 0000000..3e1a6f2 --- /dev/null +++ b/tutorials/semantic_reasoning_demo/anchoring_extra/monitors_anchoring_interface/package.xml @@ -0,0 +1,22 @@ + + + + monitors_anchoring_interface + 0.1.0 + Component that triggers the anchoring''s knowledge update process when a property violation is detected by monitors + user + Apache-2.0 + + rclpy + std_msgs + anchoring_process_interfaces + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/tutorials/semantic_reasoning_demo/anchoring_extra/monitors_anchoring_interface/resource/monitors_anchoring_interface b/tutorials/semantic_reasoning_demo/anchoring_extra/monitors_anchoring_interface/resource/monitors_anchoring_interface new file mode 100644 index 0000000..e69de29 diff --git a/tutorials/semantic_reasoning_demo/anchoring_extra/monitors_anchoring_interface/setup.cfg b/tutorials/semantic_reasoning_demo/anchoring_extra/monitors_anchoring_interface/setup.cfg new file mode 100644 index 0000000..77688ad --- /dev/null +++ b/tutorials/semantic_reasoning_demo/anchoring_extra/monitors_anchoring_interface/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/monitors_anchoring_interface +[install] +install_scripts=$base/lib/monitors_anchoring_interface diff --git a/tutorials/semantic_reasoning_demo/anchoring_extra/monitors_anchoring_interface/setup.py b/tutorials/semantic_reasoning_demo/anchoring_extra/monitors_anchoring_interface/setup.py new file mode 100644 index 0000000..0d3bf3f --- /dev/null +++ b/tutorials/semantic_reasoning_demo/anchoring_extra/monitors_anchoring_interface/setup.py @@ -0,0 +1,35 @@ +import os +from glob import glob +from setuptools import find_packages, setup + +package_name = 'monitors_anchoring_interface' + +setup( + name=package_name, + version='0.1.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + # Include launch files + ('share/'+ package_name + '/launch/cfg/', ['launch/cfg/params.yaml']), + (os.path.join('share', package_name, 'launch'), glob('launch/*.*')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Matteo MORELLI', + maintainer_email='matteo.morelli@cea.fr', + description='Component that triggers the anchoring''s knowledge update process when a property violation is detected by monitors', + license='Apache-2.0', + extras_require={ + 'test': [ + 'pytest', + ], + }, + entry_points={ + 'console_scripts': [ + "run = monitors_anchoring_interface.run:main", + ], + }, +) diff --git a/tutorials/semantic_reasoning_demo/anchoring_extra/monitors_anchoring_interface/test/test_copyright.py b/tutorials/semantic_reasoning_demo/anchoring_extra/monitors_anchoring_interface/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/tutorials/semantic_reasoning_demo/anchoring_extra/monitors_anchoring_interface/test/test_copyright.py @@ -0,0 +1,25 @@ +# 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/tutorials/semantic_reasoning_demo/anchoring_extra/monitors_anchoring_interface/test/test_flake8.py b/tutorials/semantic_reasoning_demo/anchoring_extra/monitors_anchoring_interface/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/tutorials/semantic_reasoning_demo/anchoring_extra/monitors_anchoring_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/tutorials/semantic_reasoning_demo/anchoring_extra/monitors_anchoring_interface/test/test_pep257.py b/tutorials/semantic_reasoning_demo/anchoring_extra/monitors_anchoring_interface/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/tutorials/semantic_reasoning_demo/anchoring_extra/monitors_anchoring_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' diff --git a/tutorials/semantic_reasoning_demo/anchoring_extra/tutorial_homeservice/CMakeLists.txt b/tutorials/semantic_reasoning_demo/anchoring_extra/tutorial_homeservice/CMakeLists.txt new file mode 100755 index 0000000..310506c --- /dev/null +++ b/tutorials/semantic_reasoning_demo/anchoring_extra/tutorial_homeservice/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.5.0) +project(tutorial_homeservice) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(anchoring_process REQUIRED) + +# Start of user code dependencies +# End of user code + + +# Install launch files. +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME}/ +) +ament_package() diff --git a/tutorials/semantic_reasoning_demo/anchoring_extra/tutorial_homeservice/launch/cfg/params.yaml b/tutorials/semantic_reasoning_demo/anchoring_extra/tutorial_homeservice/launch/cfg/params.yaml new file mode 100644 index 0000000..d05958d --- /dev/null +++ b/tutorials/semantic_reasoning_demo/anchoring_extra/tutorial_homeservice/launch/cfg/params.yaml @@ -0,0 +1,10 @@ +anchoring_process: + ros__parameters: + dt: + service_names: + - "/dt/get_data" + - "/robot/executor/get_info" + timeout: 2 # seconds + SkrawlMoMa: + database_name: "convince-semantic_reasoning_demo" + diff --git a/tutorials/semantic_reasoning_demo/anchoring_extra/tutorial_homeservice/launch/tutorial_homeservice.launch.py b/tutorials/semantic_reasoning_demo/anchoring_extra/tutorial_homeservice/launch/tutorial_homeservice.launch.py new file mode 100644 index 0000000..6620f68 --- /dev/null +++ b/tutorials/semantic_reasoning_demo/anchoring_extra/tutorial_homeservice/launch/tutorial_homeservice.launch.py @@ -0,0 +1,33 @@ +from launch import LaunchDescription + +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + +def generate_launch_description(): + + demo_params = PathJoinSubstitution([ + FindPackageShare('tutorial_homeservice'), + 'launch', + 'cfg', + 'params.yaml' + ]) + + return LaunchDescription([ + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare('anchoring_process'), + 'launch', + 'anchoring_process.launch.py' + ]) + ), + launch_arguments={ + 'params_file': demo_params, + 'knowledge_domain': 'SkrawlMoMa', + 'instances_setup': '/tmp/dt/setup.json' + }.items() + ) + ]) + diff --git a/tutorials/semantic_reasoning_demo/anchoring_extra/tutorial_homeservice/package.xml b/tutorials/semantic_reasoning_demo/anchoring_extra/tutorial_homeservice/package.xml new file mode 100755 index 0000000..ea3cafb --- /dev/null +++ b/tutorials/semantic_reasoning_demo/anchoring_extra/tutorial_homeservice/package.xml @@ -0,0 +1,19 @@ + + + tutorial_homeservice + 0.1.0 + Bringup launcher for using the semantic anchoring stack with the home service domain ontology + Matteo MORELLI + Matteo MORELLI + CeCILL-B + + ament_cmake + + rclcpp + rclcpp_lifecycle + anchoring_process + + + ament_cmake + + diff --git a/tutorials/semantic_reasoning_demo/imgs/pyrobosim.png b/tutorials/semantic_reasoning_demo/imgs/pyrobosim.png new file mode 100644 index 0000000..f7324d4 Binary files /dev/null and b/tutorials/semantic_reasoning_demo/imgs/pyrobosim.png differ diff --git a/tutorials/semantic_reasoning_demo/imgs/srd-bringup.png b/tutorials/semantic_reasoning_demo/imgs/srd-bringup.png new file mode 100644 index 0000000..6325c91 Binary files /dev/null and b/tutorials/semantic_reasoning_demo/imgs/srd-bringup.png differ diff --git a/tutorials/semantic_reasoning_demo/imgs/srd-bt1.png b/tutorials/semantic_reasoning_demo/imgs/srd-bt1.png new file mode 100644 index 0000000..b24202d Binary files /dev/null and b/tutorials/semantic_reasoning_demo/imgs/srd-bt1.png differ diff --git a/tutorials/semantic_reasoning_demo/imgs/srd-bt2.png b/tutorials/semantic_reasoning_demo/imgs/srd-bt2.png new file mode 100644 index 0000000..6428aef Binary files /dev/null and b/tutorials/semantic_reasoning_demo/imgs/srd-bt2.png differ diff --git a/tutorials/semantic_reasoning_demo/imgs/srd-conceptual_place_property_violation.png b/tutorials/semantic_reasoning_demo/imgs/srd-conceptual_place_property_violation.png new file mode 100644 index 0000000..65c1851 Binary files /dev/null and b/tutorials/semantic_reasoning_demo/imgs/srd-conceptual_place_property_violation.png differ diff --git a/tutorials/semantic_reasoning_demo/imgs/srd-place_precondition_ok.png b/tutorials/semantic_reasoning_demo/imgs/srd-place_precondition_ok.png new file mode 100644 index 0000000..53c078a Binary files /dev/null and b/tutorials/semantic_reasoning_demo/imgs/srd-place_precondition_ok.png differ diff --git a/tutorials/semantic_reasoning_demo/imgs/srd-violated_place_precondition.png b/tutorials/semantic_reasoning_demo/imgs/srd-violated_place_precondition.png new file mode 100644 index 0000000..057ea69 Binary files /dev/null and b/tutorials/semantic_reasoning_demo/imgs/srd-violated_place_precondition.png differ diff --git a/tutorials/semantic_reasoning_demo/misc/anchoring/dt/setup.json b/tutorials/semantic_reasoning_demo/misc/anchoring/dt/setup.json new file mode 100644 index 0000000..583221a --- /dev/null +++ b/tutorials/semantic_reasoning_demo/misc/anchoring/dt/setup.json @@ -0,0 +1,120 @@ +[ + { + "id": "_dining_table", + "name": "dining table", + "setup_properties": { + "class": "table" + }, + "DT_config": [ + { + "DigitalTwin": "pyrobosim", + "id_DT": "dining_table" + } + ] + }, + { + "id": "_side_table", + "name": "side table", + "setup_properties": { + "class": "table" + }, + "DT_config": [ + { + "DigitalTwin": "pyrobosim", + "id_DT": "side_table" + } + ] + }, + { + "id": "_kitchen_table", + "name": "kitchen table", + "setup_properties": { + "class": "table" + }, + "DT_config": [ + { + "DigitalTwin": "pyrobosim", + "id_DT": "kitchen_table" + } + ] + }, + { + "id": "_fridge", + "name": "fridge", + "setup_properties": { + "class": "storage_object" + }, + "DT_config": [ + { + "DigitalTwin": "pyrobosim", + "id_DT": "fridge" + } + ] + }, + { + "id": "_soda0", + "name": "soda 0", + "setup_properties": { + "class": "soda" + }, + "DT_config": [ + { + "DigitalTwin": "pyrobosim", + "id_DT": "soda0" + } + ] + }, + { + "id": "_snacks0", + "name": "snacks 0", + "setup_properties": { + "class": "snacks" + }, + "DT_config": [ + { + "DigitalTwin": "pyrobosim", + "id_DT": "snacks0" + } + ] + }, + { + "id": "_butter0", + "name": "butter 0", + "setup_properties": { + "class": "butter" + }, + "DT_config": [ + { + "DigitalTwin": "pyrobosim", + "id_DT": "butter0" + } + ] + }, + { + "id": "_robot", + "name": "robot", + "setup_properties": { + "class": "robot" + }, + "DT_config": [ + { + "DigitalTwin": "pyrobosim", + "id_DT": "robot" + } + ] + }, + { + "id": "_place44", + "name": "place 44", + "setup_properties": { + "class": "place" + }, + "DT_config": [ + { + "DigitalTwin": "btcpp", + "id_DT": "place44" + } + ] + } +] + diff --git a/tutorials/semantic_reasoning_demo/misc/anchoring/tql/queries/0_find_entities_positions_shapes.tql b/tutorials/semantic_reasoning_demo/misc/anchoring/tql/queries/0_find_entities_positions_shapes.tql new file mode 100644 index 0000000..82ff74a --- /dev/null +++ b/tutorials/semantic_reasoning_demo/misc/anchoring/tql/queries/0_find_entities_positions_shapes.tql @@ -0,0 +1,7 @@ +match + $e isa physical_entity, has name $en; + $hp (owner: $e, position: $p) isa has_position; + $p isa position3d, has lin_x $px, has lin_y $py, has lin_z $pz; + $hs (owner: $e, shape: $b) isa has_shape; + $b isa box_shape, has width $bw, has length $bl, has height $bh; +get; diff --git a/tutorials/semantic_reasoning_demo/misc/anchoring/tql/queries/1_find_entities_and_dispositions.tql b/tutorials/semantic_reasoning_demo/misc/anchoring/tql/queries/1_find_entities_and_dispositions.tql new file mode 100644 index 0000000..587dd77 --- /dev/null +++ b/tutorials/semantic_reasoning_demo/misc/anchoring/tql/queries/1_find_entities_and_dispositions.tql @@ -0,0 +1,5 @@ +match + $p isa physical_entity, has name $pn, has id $pid; + $hd isa has_disposition; + $hc isa has_capability; +get; diff --git a/tutorials/semantic_reasoning_demo/misc/anchoring/tql/queries/2_find_non_manifested_affordances.tql b/tutorials/semantic_reasoning_demo/misc/anchoring/tql/queries/2_find_non_manifested_affordances.tql new file mode 100644 index 0000000..030ccbc --- /dev/null +++ b/tutorials/semantic_reasoning_demo/misc/anchoring/tql/queries/2_find_non_manifested_affordances.tql @@ -0,0 +1,7 @@ +match + $nmpa (evaluated_affordance: $pa, operational_context: $sit) isa non_manifested_affordance; + $pa (describes: $cwo, describes: $dwt, describes: $dwr) isa place_affordance; + (has_context: $sit, cause: $cause) isa precondition_violation; + (assigned_role: $ar, player: $p, context: $s) isa role_assignment; + $p isa physical_entity, has name $pn; +get; diff --git a/tutorials/semantic_reasoning_demo/misc/anchoring/tql/queries/3_find_robot_is_in_front_of.tql b/tutorials/semantic_reasoning_demo/misc/anchoring/tql/queries/3_find_robot_is_in_front_of.tql new file mode 100644 index 0000000..64a113b --- /dev/null +++ b/tutorials/semantic_reasoning_demo/misc/anchoring/tql/queries/3_find_robot_is_in_front_of.tql @@ -0,0 +1,5 @@ +match + $f (located: $a, location: $p) isa is_in_front_of; + $a isa artificial_agent, has name $an; + $p isa physical_entity, has name $pn; +get; diff --git a/tutorials/semantic_reasoning_demo/misc/anchoring/tql/typedb-studio.properties b/tutorials/semantic_reasoning_demo/misc/anchoring/tql/typedb-studio.properties new file mode 100644 index 0000000..97e9f36 --- /dev/null +++ b/tutorials/semantic_reasoning_demo/misc/anchoring/tql/typedb-studio.properties @@ -0,0 +1,10 @@ +connection.tls_enabled=true +connection.use_cloud_address_translation=false +connection.server=TypeDB Core +connection.core_address=localhost\:1729 +connection.advanced_config_selected=true +project.path=/convince_ws/src/tutorials/semantic_reasoning_demo/misc/anchoring/tql +connection.cloud_addresses= +connection.ca_certificate= +connection.username= +connection.cloud_address_translation= diff --git a/tutorials/semantic_reasoning_demo/misc/moon/monitor_config.yaml b/tutorials/semantic_reasoning_demo/misc/moon/monitor_config.yaml new file mode 100644 index 0000000..fcee807 --- /dev/null +++ b/tutorials/semantic_reasoning_demo/misc/moon/monitor_config.yaml @@ -0,0 +1,21 @@ +path: /convince_ws/src/tutorials/semantic_reasoning_demo/moon_extras/src/ # path to the ros + # workspace of the + # monitor package + +monitors: # list of generated monitors + - monitor: + id: place_monitor # monitor id + log: ./log.txt # file where the monitor will log the observed events + silent: False # the monitor prints info during its execution + oracle: # the oracle running and ready to check the specification + port: 8080 # port where the oracle is listening + url: 127.0.0.1 # url where itthe oracle is listening + action: nothing # action performed by the oracle + topics: # list of topics this monitor is going to intercept + - name: robot/exec_action_name # name of the topic + type: std_msgs.msg.String # type of the topic + action: log + - name: robot/force_effort_z # name of the topic + type: std_msgs.msg.Float32 # type of the topic + action: log + diff --git a/tutorials/semantic_reasoning_demo/misc/moon/place_test-prop1.py b/tutorials/semantic_reasoning_demo/misc/moon/place_test-prop1.py new file mode 100644 index 0000000..8bad776 --- /dev/null +++ b/tutorials/semantic_reasoning_demo/misc/moon/place_test-prop1.py @@ -0,0 +1,26 @@ +import oracle + +# property to verify +# During a Place, the effort shall not exceed 4.5N for more than Delta seconds (here 200ms) +PROPERTY = "( {PLACE} -> ( once[0:0.2]( {effort <= 4.5} ) ) )" + +# declaration of predicates used in the property (initialization at time 0) +predicates = dict( + time = 0, + IDLE = True, + PICK = False, + NAVIGATE = False, + PLACE = False, + effort = 0.0, +) + +# function to abstract a dictionary (obtained from Json message) into a list of predicates +# the behavior of the function must be defined by the user depending on the property and topic/service message +def abstract_message(message): + predicates['time'] = message['time'] + predicates['effort'] = message['data'] if message['topic'] == 'robot/force_effort_z' else predicates['effort'] + predicates['IDLE'] = message['data'] == 'IDLE' if message['topic'] == 'robot/exec_action_name' else predicates['IDLE'] + predicates['PICK'] = message['data'] == 'PICK' if message['topic'] == 'robot/exec_action_name' else predicates['PICK'] + predicates['NAVIGATE'] = message['data'] == 'NAVIGATE' if message['topic'] == 'robot/exec_action_name' else predicates['NAVIGATE'] + predicates['PLACE'] = message['data'] == 'PLACE' if message['topic'] == 'robot/exec_action_name' else predicates['PLACE'] + return predicates diff --git a/tutorials/semantic_reasoning_demo/moon_extras/monitor/.packagexml b/tutorials/semantic_reasoning_demo/moon_extras/monitor/.packagexml new file mode 100644 index 0000000..c41e095 --- /dev/null +++ b/tutorials/semantic_reasoning_demo/moon_extras/monitor/.packagexml @@ -0,0 +1,25 @@ + + + + monitor + 0.0.0 + TODO: Package description + parallels + TODO: License declaration + + ament_cmake + ament_cmake_python + + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + rclpy + rosmonitoring_interfaces + + + ament_cmake + + diff --git a/tutorials/semantic_reasoning_demo/moon_extras/monitor/CMakeLists.txt b/tutorials/semantic_reasoning_demo/moon_extras/monitor/CMakeLists.txt new file mode 100644 index 0000000..fc68e29 --- /dev/null +++ b/tutorials/semantic_reasoning_demo/moon_extras/monitor/CMakeLists.txt @@ -0,0 +1,21 @@ +cmake_minimum_required(VERSION 3.5) +project(monitor) + +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) +find_package(ament_index_cpp REQUIRED) +find_package(python_cmake_module REQUIRED) +find_package(PythonExtra MODULE) + +ament_python_install_package(${PROJECT_NAME}) +set(SCRIPTS_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/scripts) +file(GLOB_RECURSE py_execs RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} ${SCRIPTS_DIRECTORY}/*) +install(PROGRAMS + ${py_execs} + DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME}) + +ament_package() diff --git a/tutorials/semantic_reasoning_demo/moon_extras/monitor/launch/monitor.launch b/tutorials/semantic_reasoning_demo/moon_extras/monitor/launch/monitor.launch new file mode 100644 index 0000000..5c90397 --- /dev/null +++ b/tutorials/semantic_reasoning_demo/moon_extras/monitor/launch/monitor.launch @@ -0,0 +1 @@ + \ No newline at end of file diff --git a/tutorials/semantic_reasoning_demo/moon_extras/monitor/monitor/__init__.py b/tutorials/semantic_reasoning_demo/moon_extras/monitor/monitor/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/tutorials/semantic_reasoning_demo/moon_extras/monitor/monitor/place_monitor.py b/tutorials/semantic_reasoning_demo/moon_extras/monitor/monitor/place_monitor.py new file mode 100644 index 0000000..bde70a9 --- /dev/null +++ b/tutorials/semantic_reasoning_demo/moon_extras/monitor/monitor/place_monitor.py @@ -0,0 +1,146 @@ +#!/usr/bin/env python + +# begin imports +import json +import yaml +import websocket +import sys +import rclpy +import rosidl_runtime_py +from rclpy.node import Node +from threading import * +from rosmonitoring_interfaces.msg import MonitorError +from std_msgs.msg import * +from rclpy.callback_groups import MutuallyExclusiveCallbackGroup +from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy +# done import + +class ROSMonitor_place_monitor(Node): + + + def callbackrobot_exec_action_name(self,data): + self.get_logger().info("monitor has observed "+ str(data)) + dict= rosidl_runtime_py.message_to_ordereddict(data) + dict['topic']='robot/exec_action_name' + dict['time']=float(self.get_clock().now().to_msg().sec) + float((self.get_clock().now().to_msg().nanosec) / 1000000000) + self.ws_lock.acquire() + while dict['time'] in self.dict_msgs: + dict['time']+=0.01 + self.ws.send(json.dumps(dict)) + self.dict_msgs[dict['time']] = data + message=self.ws.recv() + self.ws_lock.release() + self.get_logger().info("event propagated to oracle") + self.on_message_topic(message) + + def callbackrobot_force_effort_z(self,data): + self.get_logger().info("monitor has observed "+ str(data)) + dict= rosidl_runtime_py.message_to_ordereddict(data) + dict['topic']='robot/force_effort_z' + dict['time']=float(self.get_clock().now().to_msg().sec) + float((self.get_clock().now().to_msg().nanosec) / 1000000000) + self.ws_lock.acquire() + while dict['time'] in self.dict_msgs: + dict['time']+=0.01 + self.ws.send(json.dumps(dict)) + self.dict_msgs[dict['time']] = data + message=self.ws.recv() + self.ws_lock.release() + self.get_logger().info("event propagated to oracle") + self.on_message_topic(message) + + def __init__(self,monitor_name,log,actions): + self.monitor_publishers={} + self.config_publishers={} + self.config_subscribers={} + self.config_client_services={} + self.config_server_services={} + self.services_info={} + self.dict_msgs={} + self.ws_lock=Lock() + self.name=monitor_name + self.actions=actions + self.logfn=log + self.topics_info={} + super().__init__(self.name) + # creating the verdict and error publishers for the monitor + self.monitor_publishers['error']=self.create_publisher(topic=self.name+'/monitor_error',msg_type=MonitorError,qos_profile=1000) + + self.monitor_publishers['verdict']=self.create_publisher(topic=self.name+'/monitor_verdict',msg_type=String,qos_profile=1000) + + # done creating monitor publishers + + self.publish_topics=False + self.topics_info['robot/exec_action_name']={'package': 'std_msgs.msg', 'type': 'String', 'qos_reliability': 'reliable'} + self.topics_info['robot/force_effort_z']={'package': 'std_msgs.msg', 'type': 'Float32', 'qos_reliability': 'reliable'} + self.config_subscribers['robot/exec_action_name']=self.create_subscription(topic='robot/exec_action_name',msg_type=String,callback=self.callbackrobot_exec_action_name,qos_profile=QoSProfile(reliability=ReliabilityPolicy.RELIABLE, durability=DurabilityPolicy.VOLATILE, depth=1000)) + + self.config_subscribers['robot/force_effort_z']=self.create_subscription(topic='robot/force_effort_z',msg_type=Float32,callback=self.callbackrobot_force_effort_z,qos_profile=QoSProfile(reliability=ReliabilityPolicy.RELIABLE, durability=DurabilityPolicy.VOLATILE, depth=1000)) + + self.get_logger().info('Monitor' + self.name + ' started and ready' ) + self.get_logger().info('Logging at' + self.logfn ) + websocket.enableTrace(True) + self.ws = websocket.WebSocket() + self.ws.connect('ws://127.0.0.1:8080') + self.get_logger().info('Websocket is open') + + + def on_message_topic(self,message): + json_dict = json.loads(message) + verdict = str(json_dict['verdict']) + if verdict == 'true' or verdict == 'currently_true' or verdict == 'unknown': + if verdict == 'true' and not self.publish_topics: + self.get_logger().info('The monitor concluded the satisfaction of the property under analysis and can be safely removed.') + self.ws.close() + exit(0) + else: + self.logging(json_dict) + topic = json_dict['topic'] + self.get_logger().info('The event '+message+' is consistent and republished') + if topic in self.config_publishers: + self.config_publishers[topic].publish(self.dict_msgs[json_dict['time']]) + del self.dict_msgs[json_dict['time']] + else: + self.logging(json_dict) + self.get_logger().info('The event' + message + ' is inconsistent' ) + error = MonitorError() + error.m_topic = json_dict['topic'] + error.m_time = json_dict['time'] + error.m_property = json_dict['spec'] + error.m_content = str(self.dict_msgs[json_dict['time']]) + self.monitor_publishers['error'].publish(error) + if verdict == 'false' and not self.publish_topics: + self.get_logger().info('The monitor concluded the violation of the property under analysis and can be safely removed.') + self.ws.close() + exit(0) + if self.actions[json_dict['topic']][0] != 'filter': + topic = json_dict['topic'] + if topic in self.config_publishers: + self.config_publishers[topic].publish(self.dict_msgs[json_dict['time']]) + del self.dict_msgs[json_dict['time']] + error=True + verdict_msg = String() + verdict_msg.data = verdict + self.monitor_publishers['verdict'].publish(verdict_msg) + + def logging(self,json_dict): + try: + with open(self.logfn,'a+') as log_file: + log_file.write(json.dumps(json_dict)+'\n') + self.get_logger().info('Event logged') + except: + self.get_logger().info('Unable to log the event') + +def main(args=None): + rclpy.init(args=args) + log = './log.txt' + actions = {} + actions['robot/exec_action_name']=('log',0) + actions['robot/force_effort_z']=('log',0) + monitor = ROSMonitor_place_monitor('place_monitor',log,actions) + rclpy.spin(monitor) + monitor.ws.close() + monitor.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/tutorials/semantic_reasoning_demo/moon_extras/monitor/package.xml b/tutorials/semantic_reasoning_demo/moon_extras/monitor/package.xml new file mode 100644 index 0000000..5cc6a0d --- /dev/null +++ b/tutorials/semantic_reasoning_demo/moon_extras/monitor/package.xml @@ -0,0 +1,23 @@ + + monitor + 0.0.0 + TODO: Package description + parallels + TODO: License declaration + + ament_cmake + ament_cmake_python + + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + std_msgsstd_msgsrclpy + rosmonitoring_interfaces + + + ament_cmake + + \ No newline at end of file diff --git a/tutorials/semantic_reasoning_demo/moon_extras/monitor/resource/monitor b/tutorials/semantic_reasoning_demo/moon_extras/monitor/resource/monitor new file mode 100644 index 0000000..e69de29 diff --git a/tutorials/semantic_reasoning_demo/moon_extras/monitor/scripts/place_monitor b/tutorials/semantic_reasoning_demo/moon_extras/monitor/scripts/place_monitor new file mode 100755 index 0000000..822bd1e --- /dev/null +++ b/tutorials/semantic_reasoning_demo/moon_extras/monitor/scripts/place_monitor @@ -0,0 +1,6 @@ +#!/usr/bin/env python3 + +from monitor.place_monitor import main + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/tutorials/semantic_reasoning_demo/moon_extras/monitor/setup_resources.py b/tutorials/semantic_reasoning_demo/moon_extras/monitor/setup_resources.py new file mode 100644 index 0000000..4a15d1b --- /dev/null +++ b/tutorials/semantic_reasoning_demo/moon_extras/monitor/setup_resources.py @@ -0,0 +1,40 @@ +# so that we can get the the scripts path +# so that we can get the the scripts path +import os + + +# helper function for getting the files +def get_files(root_path,ext,prefix): + all_files = [] + for root, dirs, files in os.walk(root_path): + for filename in files: + if filename.lower().endswith(ext) and filename.lower().startswith(prefix): + if filename != '__init__.py': + all_files.append( filename ) + return all_files + +def script_name(sf): + sfname = sf.replace("default_","") + sfname = sfname.replace("default","") + sfname = sfname.replace(".py","") + return sfname + +def create_entrypoint_line(sf,package_name): + sfname = script_name(sf) + line = sfname+' = '+package_name+'.'+sf.replace(".py",":main") + return line + +def console_script_lines(scriptfiles,package_name): + sflines = [] + for sf in scriptfiles: + line = create_entrypoint_line(sf,package_name) + sflines.append(line) + return sflines + + + +def get_console_script_lines(package_name): + script_path = os.path.dirname(os.path.realpath(__file__)) + scriptfiles = get_files(script_path+'/'+package_name+"/" ,".py","") + sflines = console_script_lines(scriptfiles,package_name) + return sflines \ No newline at end of file diff --git a/tutorials/semantic_reasoning_demo/moon_extras/rosmonitoring_interfaces/CMakeLists.txt b/tutorials/semantic_reasoning_demo/moon_extras/rosmonitoring_interfaces/CMakeLists.txt new file mode 100644 index 0000000..f671ba4 --- /dev/null +++ b/tutorials/semantic_reasoning_demo/moon_extras/rosmonitoring_interfaces/CMakeLists.txt @@ -0,0 +1,38 @@ +cmake_minimum_required(VERSION 3.8) +project(rosmonitoring_interfaces) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +find_package(rosidl_default_generators REQUIRED) + +set(msg_files + "msg/MonitorError.msg" +) + +rosidl_generate_interfaces(${PROJECT_NAME} + ${msg_files} +) + +ament_export_dependencies(rosidl_default_runtime) + + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/tutorials/semantic_reasoning_demo/moon_extras/rosmonitoring_interfaces/msg/MonitorError.msg b/tutorials/semantic_reasoning_demo/moon_extras/rosmonitoring_interfaces/msg/MonitorError.msg new file mode 100644 index 0000000..ef2b453 --- /dev/null +++ b/tutorials/semantic_reasoning_demo/moon_extras/rosmonitoring_interfaces/msg/MonitorError.msg @@ -0,0 +1,5 @@ +string m_topic +string m_service +string m_content +string m_property +float64 m_time \ No newline at end of file diff --git a/tutorials/semantic_reasoning_demo/moon_extras/rosmonitoring_interfaces/package.xml b/tutorials/semantic_reasoning_demo/moon_extras/rosmonitoring_interfaces/package.xml new file mode 100644 index 0000000..c97d44b --- /dev/null +++ b/tutorials/semantic_reasoning_demo/moon_extras/rosmonitoring_interfaces/package.xml @@ -0,0 +1,24 @@ + + + + rosmonitoring_interfaces + 0.0.0 + TODO: Package description + parallels + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + rosidl_default_generators + + rosidl_default_runtime + + rosidl_interface_packages + + + ament_cmake + + diff --git a/tutorials/semantic_reasoning_demo/semantic_reasoning_bringup/CMakeLists.txt b/tutorials/semantic_reasoning_demo/semantic_reasoning_bringup/CMakeLists.txt new file mode 100755 index 0000000..26aa1ff --- /dev/null +++ b/tutorials/semantic_reasoning_demo/semantic_reasoning_bringup/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.5.0) +project(semantic_reasoning_bringup) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +#find_package(anchoring_process REQUIRED) + +# Start of user code dependencies +# End of user code + + +# Install launch files. +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME}/ +) +ament_package() diff --git a/tutorials/semantic_reasoning_demo/semantic_reasoning_bringup/launch/bringup.launch.py b/tutorials/semantic_reasoning_demo/semantic_reasoning_bringup/launch/bringup.launch.py new file mode 100644 index 0000000..23515e2 --- /dev/null +++ b/tutorials/semantic_reasoning_demo/semantic_reasoning_bringup/launch/bringup.launch.py @@ -0,0 +1,132 @@ +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + IncludeLaunchDescription, + SetEnvironmentVariable, + ExecuteProcess, +) +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +import os +from ament_index_python.packages import get_package_share_directory + +def generate_launch_description(): + + # simulated world, skill executor and its digital twin + # ==================================================== + skill_executor_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + PathJoinSubstitution([ + FindPackageShare('tutorial_skill_executor'), + 'launch', + 'tutorial_skill_executor.launch.py' + ]) + ]) + ) + + dt = Node( + package="tutorial_dt", + executable="run", + name="tutorial_dt", + output="screen", + emulate_tty=True, + ) + + # moon + # ==== + moon_oracle = ExecuteProcess( + # moon_oracle --online --property /semantic_reasoning_demo/config/moon/place_test-prop1.py --port 8080 --dense + cmd = [ + 'moon_oracle', + '--online', + '--property', + '/convince_ws/src/tutorials/semantic_reasoning_demo/misc/moon/place_test-prop1.py', + '--port', + '8080', + '--dense' + ], + shell=True, + output='screen' + ) + + place_monitor = Node( + package="monitor", + executable="place_monitor", + name="monitor", + output="screen", + emulate_tty=True, + ) + + monitor_anchoring_params = PathJoinSubstitution([ + FindPackageShare('monitors_anchoring_interface'), + 'launch', + 'cfg', + 'params.yaml' + ]) + + monitors_anchoring_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare('monitors_anchoring_interface'), + 'launch', + 'monitors_anchoring_interface.launch.py' + ]) + ), + launch_arguments={ + 'params_file': monitor_anchoring_params + }.items() + ) + + # force-data inspection + # ===================== + plotjuggler = ExecuteProcess( + cmd = [ + 'ros2', + 'run', + 'plotjuggler', + 'plotjuggler', + '--nosplash', + '--buffer_size', + '999' + ], + shell=True, + output='screen' + ) + + # semantic anchoring and reasoner + # =============================== + anchoring_params = PathJoinSubstitution([ + FindPackageShare('tutorial_homeservice'), + 'launch', + 'cfg', + 'params.yaml' + ]) + + anchoring_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare('anchoring_process'), + 'launch', + 'anchoring_process.launch.py' + ]) + ), + launch_arguments={ + 'params_file': anchoring_params, + 'knowledge_domain': 'SkrawlMoMa', + 'instances_setup': '/convince_ws/src/tutorials/semantic_reasoning_demo/misc/anchoring/dt/setup.json' + }.items() + ) + + return LaunchDescription( + [ + skill_executor_launch, + dt, + moon_oracle, + place_monitor, + monitors_anchoring_launch, + plotjuggler, + anchoring_launch, + ] + ) diff --git a/tutorials/semantic_reasoning_demo/semantic_reasoning_bringup/launch/cfg/params.yaml b/tutorials/semantic_reasoning_demo/semantic_reasoning_bringup/launch/cfg/params.yaml new file mode 100644 index 0000000..f198641 --- /dev/null +++ b/tutorials/semantic_reasoning_demo/semantic_reasoning_bringup/launch/cfg/params.yaml @@ -0,0 +1,12 @@ +bt_executor2: + ros__parameters: + action_name: "/robot/executor/execute_behavior" # 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: + - bt_executor2/bt_plugins + + behavior_trees: + - bt_executor2/behavior_trees diff --git a/tutorials/semantic_reasoning_demo/semantic_reasoning_bringup/launch/policy_executor.launch.py b/tutorials/semantic_reasoning_demo/semantic_reasoning_bringup/launch/policy_executor.launch.py new file mode 100644 index 0000000..74b6b97 --- /dev/null +++ b/tutorials/semantic_reasoning_demo/semantic_reasoning_bringup/launch/policy_executor.launch.py @@ -0,0 +1,83 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import ( + RegisterEventHandler, + ExecuteProcess, + DeclareLaunchArgument, + TimerAction, + EmitEvent +) +from launch.event_handlers import OnProcessStart, OnProcessExit +from launch.events import Shutdown +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + +def generate_launch_description(): + + bt_executor_params = PathJoinSubstitution([ + FindPackageShare('semantic_reasoning_bringup'), + 'launch', + 'cfg', + 'params.yaml' + ]) + + target_tree = LaunchConfiguration('target_tree') + + target_tree_arg = DeclareLaunchArgument( + 'target_tree', + default_value='SRBT2', + description='Behavior tree name' + ) + + bt_executor = Node( + package="bt_executor2", + executable="bt_executor2", + name="bt_executor2", + output="screen", + emulate_tty=True, + parameters=[bt_executor_params], + ) + + send_goal_cmd = ExecuteProcess( + cmd=[ + 'ros2', 'action', 'send_goal', + '/robot/executor/execute_behavior', + 'btcpp_ros2_interfaces/action/ExecuteTree', + ['\"{target_tree: \'', target_tree, '\'}\"'] + ], + shell=True, + output='screen' + ) + + delayed_send_goal = TimerAction( + period=2.0, + actions=[send_goal_cmd], + ) + + trigger_after_start = RegisterEventHandler( + OnProcessStart( + target_action=bt_executor, + on_start=[delayed_send_goal], + ) + ) + + # When send_goal finishes -> shutdown everything + shutdown_on_goal_exit = RegisterEventHandler( + OnProcessExit( + target_action=send_goal_cmd, + on_exit=[ + TimerAction( + period=1.0, + actions=[EmitEvent(event=Shutdown(reason='Behavior execution done'))] + ) + ], + ) + ) + + return LaunchDescription([ + target_tree_arg, + bt_executor, + trigger_after_start, + shutdown_on_goal_exit, + ]) + diff --git a/tutorials/semantic_reasoning_demo/semantic_reasoning_bringup/package.xml b/tutorials/semantic_reasoning_demo/semantic_reasoning_bringup/package.xml new file mode 100755 index 0000000..b3be22f --- /dev/null +++ b/tutorials/semantic_reasoning_demo/semantic_reasoning_bringup/package.xml @@ -0,0 +1,20 @@ + + + semantic_reasoning_bringup + 0.1.0 + Bringup launcher for the semantic reasoning demo in Convince Overarching Tutorial + Matteo MORELLI + Matteo MORELLI + CeCILL-B + + ament_cmake + + rclcpp + rclcpp_lifecycle + + tutorial_skill_executor + + + ament_cmake + + diff --git a/tutorials/semantic_reasoning_demo/simulation_extras/bt_executor2/CMakeLists.txt b/tutorials/semantic_reasoning_demo/simulation_extras/bt_executor2/CMakeLists.txt new file mode 100644 index 0000000..b79ac1f --- /dev/null +++ b/tutorials/semantic_reasoning_demo/simulation_extras/bt_executor2/CMakeLists.txt @@ -0,0 +1,68 @@ +cmake_minimum_required(VERSION 3.16) +project(bt_executor2) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + + +find_package(ament_cmake REQUIRED) +find_package(behaviortree_ros2 REQUIRED) +find_package(overarching_msgs REQUIRED) + +set(THIS_PACKAGE_DEPS + behaviortree_ros2::behaviortree_ros2 + ${overarching_msgs_TARGETS} ) + +###################################################### +# Implem that uses and customizes the BtExecutionServer +add_executable(bt_executor2 src/bt_executor2.cpp) +target_link_libraries(bt_executor2 ${THIS_PACKAGE_DEPS}) + +###################################################### +# Actions (Plugins) + +add_library(pick_plugin SHARED src/pick_action.cpp) +target_compile_definitions(pick_plugin PRIVATE BT_PLUGIN_EXPORT ) +target_link_libraries(pick_plugin ${THIS_PACKAGE_DEPS}) +add_library(place_plugin SHARED src/place_action.cpp) +target_compile_definitions(place_plugin PRIVATE BT_PLUGIN_EXPORT ) +target_link_libraries(place_plugin ${THIS_PACKAGE_DEPS}) +add_library(navigate_plugin SHARED src/navigate_action.cpp) +target_compile_definitions(navigate_plugin PRIVATE BT_PLUGIN_EXPORT ) +target_link_libraries(navigate_plugin ${THIS_PACKAGE_DEPS}) + + +###################################################### +# INSTALL + +install(TARGETS + pick_plugin + place_plugin + navigate_plugin + bt_executor2 + DESTINATION lib/${PROJECT_NAME} + ) + +###################################################### +# INSTALL plugins for other packages to load + +install(TARGETS + pick_plugin + place_plugin + navigate_plugin + LIBRARY DESTINATION share/${PROJECT_NAME}/bt_plugins + ARCHIVE DESTINATION share/${PROJECT_NAME}/bt_plugins + RUNTIME DESTINATION share/${PROJECT_NAME}/bt_plugins + ) + +###################################################### +# INSTALL Behavior.xml's + +install(DIRECTORY + behavior_trees + DESTINATION share/${PROJECT_NAME}/ + ) + +ament_export_dependencies(behaviortree_ros2 overarching_msgs) + +ament_package() diff --git a/tutorials/semantic_reasoning_demo/simulation_extras/bt_executor2/behavior_trees/bt_tree_1.xml b/tutorials/semantic_reasoning_demo/simulation_extras/bt_executor2/behavior_trees/bt_tree_1.xml new file mode 100644 index 0000000..700d600 --- /dev/null +++ b/tutorials/semantic_reasoning_demo/simulation_extras/bt_executor2/behavior_trees/bt_tree_1.xml @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/tutorials/semantic_reasoning_demo/simulation_extras/bt_executor2/behavior_trees/bt_tree_2.xml b/tutorials/semantic_reasoning_demo/simulation_extras/bt_executor2/behavior_trees/bt_tree_2.xml new file mode 100644 index 0000000..170465d --- /dev/null +++ b/tutorials/semantic_reasoning_demo/simulation_extras/bt_executor2/behavior_trees/bt_tree_2.xml @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/tutorials/semantic_reasoning_demo/simulation_extras/bt_executor2/behavior_trees/bt_tree_3.xml b/tutorials/semantic_reasoning_demo/simulation_extras/bt_executor2/behavior_trees/bt_tree_3.xml new file mode 100644 index 0000000..9d4f38f --- /dev/null +++ b/tutorials/semantic_reasoning_demo/simulation_extras/bt_executor2/behavior_trees/bt_tree_3.xml @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/tutorials/semantic_reasoning_demo/simulation_extras/bt_executor2/package.xml b/tutorials/semantic_reasoning_demo/simulation_extras/bt_executor2/package.xml new file mode 100644 index 0000000..84beedd --- /dev/null +++ b/tutorials/semantic_reasoning_demo/simulation_extras/bt_executor2/package.xml @@ -0,0 +1,22 @@ + + bt_executor2 + 0.3.0 + + Simple BT executor. + + + Matteo Morelli + MIT + Davide Faconti + Matteo Morelli + + ament_cmake + + behaviortree_ros2 + overarching_msgs + + + ament_cmake + + + diff --git a/tutorials/semantic_reasoning_demo/simulation_extras/bt_executor2/src/bt_executor2.cpp b/tutorials/semantic_reasoning_demo/simulation_extras/bt_executor2/src/bt_executor2.cpp new file mode 100644 index 0000000..0074e85 --- /dev/null +++ b/tutorials/semantic_reasoning_demo/simulation_extras/bt_executor2/src/bt_executor2.cpp @@ -0,0 +1,67 @@ +#include +#include +#include + +class TreeExecutionServerWhStdCoutLogger : public BT::TreeExecutionServer +{ +public: + TreeExecutionServerWhStdCoutLogger(const rclcpp::NodeOptions& options) + : TreeExecutionServer(options) + {} + + void onTreeCreated(BT::Tree& tree) override + { + logger_cout_ = std::make_unique(tree); + } + + std::optional onTreeExecutionCompleted( + BT::NodeStatus /*status*/, + bool /*was_cancelled*/) override + { + cleanup(); + return std::nullopt; + } + + ~TreeExecutionServerWhStdCoutLogger() + { + cleanup(); + } + + void cleanup() + { + if (logger_cout_) { + logger_cout_.reset(); + } + } + +private: + std::unique_ptr logger_cout_; +}; + +int main(int argc, char* argv[]) +{ + rclcpp::init(argc, argv); + + rclcpp::NodeOptions options; + auto action_server = + std::make_shared(options); + + // Executor (same as your original) + rclcpp::executors::MultiThreadedExecutor exec( + rclcpp::ExecutorOptions(), 0, false, + std::chrono::milliseconds(250)); + + exec.add_node(action_server->node()); + + // shutdown hook + rclcpp::on_shutdown([action_server]() { + action_server->cleanup(); + }); + + exec.spin(); + + exec.remove_node(action_server->node()); + + rclcpp::shutdown(); + return 0; +} diff --git a/tutorials/semantic_reasoning_demo/simulation_extras/bt_executor2/src/navigate_action.cpp b/tutorials/semantic_reasoning_demo/simulation_extras/bt_executor2/src/navigate_action.cpp new file mode 100644 index 0000000..ba8509f --- /dev/null +++ b/tutorials/semantic_reasoning_demo/simulation_extras/bt_executor2/src/navigate_action.cpp @@ -0,0 +1,29 @@ +#include "navigate_action.hpp" +#include "behaviortree_ros2/plugins.hpp" + +bool NavigateAction::setGoal(RosActionNode::Goal& goal) +{ + auto location_id = getInput("location_id"); + goal.location_id = location_id.value(); + return true; +} + +NodeStatus NavigateAction::onResultReceived(const RosActionNode::WrappedResult& wr) +{ + return NodeStatus::SUCCESS; +} + +NodeStatus NavigateAction::onFailure(ActionNodeErrorCode error) +{ + RCLCPP_ERROR(logger(), "%s: onFailure with error: %s", name().c_str(), toStr(error)); + return NodeStatus::FAILURE; +} + +void NavigateAction::onHalt() +{ + RCLCPP_INFO(logger(), "%s: onHalt", name().c_str()); +} + +// Plugin registration. +// The class NavigateAction will self register with name "NavigateToLocation". +CreateRosNodePlugin(NavigateAction, "NavigateToLocation"); diff --git a/tutorials/semantic_reasoning_demo/simulation_extras/bt_executor2/src/navigate_action.hpp b/tutorials/semantic_reasoning_demo/simulation_extras/bt_executor2/src/navigate_action.hpp new file mode 100644 index 0000000..264c11f --- /dev/null +++ b/tutorials/semantic_reasoning_demo/simulation_extras/bt_executor2/src/navigate_action.hpp @@ -0,0 +1,26 @@ +#include "behaviortree_ros2/bt_action_node.hpp" +#include "overarching_msgs/action/navigate.hpp" + +using namespace BT; + +class NavigateAction : public RosActionNode +{ +public: + NavigateAction(const std::string& name, const NodeConfig& conf, + const RosNodeParams& params) + : RosActionNode(name, conf, params) + {} + + static BT::PortsList providedPorts() + { + return providedBasicPorts({ InputPort("location_id") }); + } + + bool setGoal(Goal& goal) override; + + void onHalt() override; + + BT::NodeStatus onResultReceived(const WrappedResult& wr) override; + + virtual BT::NodeStatus onFailure(ActionNodeErrorCode error) override; +}; diff --git a/tutorials/semantic_reasoning_demo/simulation_extras/bt_executor2/src/pick_action.cpp b/tutorials/semantic_reasoning_demo/simulation_extras/bt_executor2/src/pick_action.cpp new file mode 100644 index 0000000..5c14e0b --- /dev/null +++ b/tutorials/semantic_reasoning_demo/simulation_extras/bt_executor2/src/pick_action.cpp @@ -0,0 +1,29 @@ +#include "pick_action.hpp" +#include "behaviortree_ros2/plugins.hpp" + +bool PickAction::setGoal(RosActionNode::Goal& goal) +{ + auto object_id = getInput("object_id"); + goal.object_id = object_id.value(); + return true; +} + +NodeStatus PickAction::onResultReceived(const RosActionNode::WrappedResult& wr) +{ + return NodeStatus::SUCCESS; +} + +NodeStatus PickAction::onFailure(ActionNodeErrorCode error) +{ + RCLCPP_ERROR(logger(), "%s: onFailure with error: %s", name().c_str(), toStr(error)); + return NodeStatus::FAILURE; +} + +void PickAction::onHalt() +{ + RCLCPP_INFO(logger(), "%s: onHalt", name().c_str()); +} + +// Plugin registration. +// The class PickAction will self register with name "PickObject". +CreateRosNodePlugin(PickAction, "PickObject"); diff --git a/tutorials/semantic_reasoning_demo/simulation_extras/bt_executor2/src/pick_action.hpp b/tutorials/semantic_reasoning_demo/simulation_extras/bt_executor2/src/pick_action.hpp new file mode 100644 index 0000000..addb28e --- /dev/null +++ b/tutorials/semantic_reasoning_demo/simulation_extras/bt_executor2/src/pick_action.hpp @@ -0,0 +1,26 @@ +#include "behaviortree_ros2/bt_action_node.hpp" +#include "overarching_msgs/action/pick.hpp" + +using namespace BT; + +class PickAction : public RosActionNode +{ +public: + PickAction(const std::string& name, const NodeConfig& conf, + const RosNodeParams& params) + : RosActionNode(name, conf, params) + {} + + static BT::PortsList providedPorts() + { + return providedBasicPorts({ InputPort("object_id") }); + } + + bool setGoal(Goal& goal) override; + + void onHalt() override; + + BT::NodeStatus onResultReceived(const WrappedResult& wr) override; + + virtual BT::NodeStatus onFailure(ActionNodeErrorCode error) override; +}; diff --git a/tutorials/semantic_reasoning_demo/simulation_extras/bt_executor2/src/place_action.cpp b/tutorials/semantic_reasoning_demo/simulation_extras/bt_executor2/src/place_action.cpp new file mode 100644 index 0000000..c295079 --- /dev/null +++ b/tutorials/semantic_reasoning_demo/simulation_extras/bt_executor2/src/place_action.cpp @@ -0,0 +1,27 @@ +#include "place_action.hpp" +#include "behaviortree_ros2/plugins.hpp" + +bool PlaceAction::setGoal(RosActionNode::Goal& goal) +{ + return true; +} + +NodeStatus PlaceAction::onResultReceived(const RosActionNode::WrappedResult& wr) +{ + return NodeStatus::SUCCESS; +} + +NodeStatus PlaceAction::onFailure(ActionNodeErrorCode error) +{ + RCLCPP_ERROR(logger(), "%s: onFailure with error: %s", name().c_str(), toStr(error)); + return NodeStatus::FAILURE; +} + +void PlaceAction::onHalt() +{ + RCLCPP_INFO(logger(), "%s: onHalt", name().c_str()); +} + +// Plugin registration. +// The class PlaceAction will self register with name "PlaceObject". +CreateRosNodePlugin(PlaceAction, "PlaceObject"); diff --git a/tutorials/semantic_reasoning_demo/simulation_extras/bt_executor2/src/place_action.hpp b/tutorials/semantic_reasoning_demo/simulation_extras/bt_executor2/src/place_action.hpp new file mode 100644 index 0000000..9634f62 --- /dev/null +++ b/tutorials/semantic_reasoning_demo/simulation_extras/bt_executor2/src/place_action.hpp @@ -0,0 +1,26 @@ +#include "behaviortree_ros2/bt_action_node.hpp" +#include "overarching_msgs/action/place.hpp" + +using namespace BT; + +class PlaceAction : public RosActionNode +{ +public: + PlaceAction(const std::string& name, const NodeConfig& conf, + const RosNodeParams& params) + : RosActionNode(name, conf, params) + {} + + static BT::PortsList providedPorts() + { + return providedBasicPorts({}); + } + + bool setGoal(Goal& goal) override; + + void onHalt() override; + + BT::NodeStatus onResultReceived(const WrappedResult& wr) override; + + virtual BT::NodeStatus onFailure(ActionNodeErrorCode error) override; +}; diff --git a/tutorials/semantic_reasoning_demo/simulation_extras/tutorial_dt/package.xml b/tutorials/semantic_reasoning_demo/simulation_extras/tutorial_dt/package.xml new file mode 100644 index 0000000..97ea857 --- /dev/null +++ b/tutorials/semantic_reasoning_demo/simulation_extras/tutorial_dt/package.xml @@ -0,0 +1,22 @@ + + + + tutorial_dt + 0.1.0 + The digital twin that provides geometric information of scene objects to the anchoring process + Matteo MORELLI + CeCILL-B + + rclpy + std_srvs + pyrobosim_msgs + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/tutorials/semantic_reasoning_demo/simulation_extras/tutorial_dt/resource/tutorial_dt b/tutorials/semantic_reasoning_demo/simulation_extras/tutorial_dt/resource/tutorial_dt new file mode 100644 index 0000000..e69de29 diff --git a/tutorials/semantic_reasoning_demo/simulation_extras/tutorial_dt/setup.cfg b/tutorials/semantic_reasoning_demo/simulation_extras/tutorial_dt/setup.cfg new file mode 100644 index 0000000..f0e8d33 --- /dev/null +++ b/tutorials/semantic_reasoning_demo/simulation_extras/tutorial_dt/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/tutorial_dt +[install] +install_scripts=$base/lib/tutorial_dt diff --git a/tutorials/semantic_reasoning_demo/simulation_extras/tutorial_dt/setup.py b/tutorials/semantic_reasoning_demo/simulation_extras/tutorial_dt/setup.py new file mode 100644 index 0000000..74a662f --- /dev/null +++ b/tutorials/semantic_reasoning_demo/simulation_extras/tutorial_dt/setup.py @@ -0,0 +1,30 @@ +from setuptools import find_packages, setup + +package_name = 'tutorial_dt' + +setup( + name=package_name, + version='0.1.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='Matteo MORELLI', + maintainer_email='matteo.morelli@cea.fr', + description='The digital twin that provides geometric information of scene objects to the anchoring process', + license='CeCILL-B', + extras_require={ + 'test': [ + 'pytest', + ], + }, + entry_points={ + 'console_scripts': [ + "run = tutorial_dt.run:main", + ], + }, +) diff --git a/tutorials/semantic_reasoning_demo/simulation_extras/tutorial_dt/test/test_copyright.py b/tutorials/semantic_reasoning_demo/simulation_extras/tutorial_dt/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/tutorials/semantic_reasoning_demo/simulation_extras/tutorial_dt/test/test_copyright.py @@ -0,0 +1,25 @@ +# 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/tutorials/semantic_reasoning_demo/simulation_extras/tutorial_dt/test/test_flake8.py b/tutorials/semantic_reasoning_demo/simulation_extras/tutorial_dt/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/tutorials/semantic_reasoning_demo/simulation_extras/tutorial_dt/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/tutorials/semantic_reasoning_demo/simulation_extras/tutorial_dt/test/test_pep257.py b/tutorials/semantic_reasoning_demo/simulation_extras/tutorial_dt/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/tutorials/semantic_reasoning_demo/simulation_extras/tutorial_dt/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/tutorials/semantic_reasoning_demo/simulation_extras/tutorial_dt/tutorial_dt/__init__.py b/tutorials/semantic_reasoning_demo/simulation_extras/tutorial_dt/tutorial_dt/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/tutorials/semantic_reasoning_demo/simulation_extras/tutorial_dt/tutorial_dt/run.py b/tutorials/semantic_reasoning_demo/simulation_extras/tutorial_dt/tutorial_dt/run.py new file mode 100644 index 0000000..5141c57 --- /dev/null +++ b/tutorials/semantic_reasoning_demo/simulation_extras/tutorial_dt/tutorial_dt/run.py @@ -0,0 +1,180 @@ +import time +import rclpy +from rclpy.node import Node +from rclpy.callback_groups import MutuallyExclusiveCallbackGroup +from rclpy.executors import MultiThreadedExecutor +from std_srvs.srv import Trigger +from pyrobosim_msgs.srv import RequestWorldState +from pyrobosim_msgs.msg import WorldState +import json + +class Dimensions: + def __init__(self, width, length, height): + self.width = width + self.length = length + self.height = height + +class TutorialDigitalTwin(Node): + def __init__(self): + super().__init__('tutorial_digital_twin') + + # TODO, make it avail from yaml file or pyrobosim api + self._dimensions_map = { + 'table': Dimensions(width=1.20, length=0.80, height=0.50), + 'storage': Dimensions(width=1.00, length=0.60, height=0.50), + 'snacks': Dimensions(width=0.12, length=0.15, height=0.05), + 'soda': Dimensions(width=0.12, length=0.15, height=0.05), + 'butter': Dimensions(width=0.10, length=0.15, height=0.05), + 'robot': Dimensions(width=0.10, length=0.10, height=0.10), + } + # Create callback groups for server and client + self._get_data_service_cb_group = MutuallyExclusiveCallbackGroup() + self._world_state_client_cb_group = MutuallyExclusiveCallbackGroup() + + # Create the /dt/get_data service + self._get_data_srv = self.create_service( + Trigger, + '/dt/get_data', + self._get_data_callback, + callback_group=self._get_data_service_cb_group + ) + + # Create a client for the /request_world_state service + self._world_state_client = self.create_client( + RequestWorldState, + '/request_world_state', + callback_group=self._world_state_client_cb_group + ) + + # Wait for the /request_world_state service to be available + while not self._world_state_client.wait_for_service(timeout_sec=1.0): + self.get_logger().info('Waiting for /request_world_state service...') + + def _serialize_world_state(self, world_state: WorldState) -> str: + """ + Serialize the locations and objects information from a WorldState message into JSON format. + + Args: + world_state (WorldState): The WorldState message to serialize. + + Returns: + str: The JSON string containing the serialized locations and objects information. + """ + # Create a list to hold the flattened data + flattened_data = [] + + # Serialize locations + for location in world_state.locations: + dimens_info = self._dimensions_map[location.category] + flattened_location = { + 'dt_id': location.name, + 'position': [ + round(location.pose.position.x, 3), + round(location.pose.position.y, 3), + round(location.pose.position.z, 3) + ], + 'orientation': [ + round(location.pose.orientation.x, 3), + round(location.pose.orientation.y, 3), + round(location.pose.orientation.z, 3), + round(location.pose.orientation.w, 3) + ], + 'bounding_box': { + 'width' : dimens_info.width, + 'length': dimens_info.length, + 'height': dimens_info.height + }, + } + flattened_data.append(flattened_location) + + # Serialize objects + for obj in world_state.objects: + dimens_info = self._dimensions_map[obj.category] + flattened_object = { + 'dt_id': obj.name, +# 'type': obj.category, + 'position': [ + round(obj.pose.position.x, 3), + round(obj.pose.position.y, 3), + round(obj.pose.position.z, 3) + ], + 'orientation': [ + round(obj.pose.orientation.x, 3), + round(obj.pose.orientation.y, 3), + round(obj.pose.orientation.z, 3), + round(obj.pose.orientation.w, 3) + ], + 'bounding_box': { + 'width' : dimens_info.width, + 'length': dimens_info.length, + 'height': dimens_info.height + }, + } + flattened_data.append(flattened_object) + + # Serialize agents + for robot in world_state.robots: + dimens_info = self._dimensions_map[robot.name] + flattened_robot = { + 'dt_id': robot.name, + 'position': [ + round(robot.pose.position.x, 3), + round(robot.pose.position.y, 3), + round(robot.pose.position.z, 3) + ], + 'orientation': [ + round(robot.pose.orientation.x, 3), + round(robot.pose.orientation.y, 3), + round(robot.pose.orientation.z, 3), + round(robot.pose.orientation.w, 3) + ], + 'bounding_box': { + 'width' : dimens_info.width, + 'length': dimens_info.length, + 'height': dimens_info.height + }, + } + flattened_data.append(flattened_robot) + + # Convert the list to a JSON string + json_str = json.dumps(flattened_data, indent=2) + + return json_str + + def _get_data_callback(self, request, response): + + # Call the /request_world_state service synchronously + req = RequestWorldState.Request() + res = self._world_state_client.call(req) + + # Serialize the world state data to JSON + json_data = self._serialize_world_state(res.state) + + # Set the response + response.success = True + response.message = json_data + + return response + +def main(args=None): + rclpy.init(args=args) + + # Create an instance of the TutorialDigitalTwin node + node = TutorialDigitalTwin() + + # Use a MultiThreadedExecutor to handle the reentrant callback group + executor = MultiThreadedExecutor() + executor.add_node(node) + + try: + # Spin the node using the executor + executor.spin() + except KeyboardInterrupt: + pass + finally: + # Clean up + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/tutorials/semantic_reasoning_demo/simulation_extras/tutorial_skill_executor/launch/tutorial_skill_executor.launch.py b/tutorials/semantic_reasoning_demo/simulation_extras/tutorial_skill_executor/launch/tutorial_skill_executor.launch.py new file mode 100644 index 0000000..aab9e55 --- /dev/null +++ b/tutorials/semantic_reasoning_demo/simulation_extras/tutorial_skill_executor/launch/tutorial_skill_executor.launch.py @@ -0,0 +1,53 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, SetEnvironmentVariable +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +import os +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + + # Declare launch argument for world file + world_file_arg = DeclareLaunchArgument( + 'world_file', + default_value=os.path.join( + get_package_share_directory("tutorial_skill_executor"), + "worlds", + "world.yaml", + ), + description='Path to the world file for pyrobosim' + ) + + # Get the world file launch configuration + world_file = LaunchConfiguration('world_file') + + # Include the pyrobosim_ros demo launch file + pyrobosim_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + PathJoinSubstitution([ + FindPackageShare('pyrobosim_ros'), + 'launch', + 'demo.launch.py' + ]) + ]), + launch_arguments={ + 'world_file': world_file + }.items() + ) + + # Run the tutorial_skill_executor node in the default domain + tutorial_skill_executor_node = Node( + package='tutorial_skill_executor', + executable='run', + name='tutorial_skill_executor', + output='screen' + ) + + return LaunchDescription([ + world_file_arg, + pyrobosim_launch, + tutorial_skill_executor_node + ]) diff --git a/tutorials/semantic_reasoning_demo/simulation_extras/tutorial_skill_executor/package.xml b/tutorials/semantic_reasoning_demo/simulation_extras/tutorial_skill_executor/package.xml new file mode 100644 index 0000000..7364c49 --- /dev/null +++ b/tutorials/semantic_reasoning_demo/simulation_extras/tutorial_skill_executor/package.xml @@ -0,0 +1,21 @@ + + + + tutorial_skill_executor + 0.1.0 + Example worlds for the CONVINCE overaching tutorial based on the work of Christian Henkel, christian.henkel2@de.bosch.com. + Matteo Morelli + Apache-2.0 + + std_srvs + pyrobosim_ros + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/tutorials/semantic_reasoning_demo/simulation_extras/tutorial_skill_executor/resource/tutorial_skill_executor b/tutorials/semantic_reasoning_demo/simulation_extras/tutorial_skill_executor/resource/tutorial_skill_executor new file mode 100644 index 0000000..e69de29 diff --git a/tutorials/semantic_reasoning_demo/simulation_extras/tutorial_skill_executor/setup.cfg b/tutorials/semantic_reasoning_demo/simulation_extras/tutorial_skill_executor/setup.cfg new file mode 100644 index 0000000..9fd67f1 --- /dev/null +++ b/tutorials/semantic_reasoning_demo/simulation_extras/tutorial_skill_executor/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/tutorial_skill_executor +[install] +install_scripts=$base/lib/tutorial_skill_executor diff --git a/tutorials/semantic_reasoning_demo/simulation_extras/tutorial_skill_executor/setup.py b/tutorials/semantic_reasoning_demo/simulation_extras/tutorial_skill_executor/setup.py new file mode 100644 index 0000000..ffc628e --- /dev/null +++ b/tutorials/semantic_reasoning_demo/simulation_extras/tutorial_skill_executor/setup.py @@ -0,0 +1,31 @@ +import os +from glob import glob + +from setuptools import find_packages, setup + +package_name = "tutorial_skill_executor" + +setup( + name=package_name, + version="0.1.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 + "/worlds", glob("worlds/*.*")), + # Include launch files + (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="Matteo MORELLI", + maintainer_email="matteo.morelli@cea.fr", + description="Example worlds for the CONVINCE overaching tutorial based on the work of Christian Henkel, christian.henkel2@de.bosch.com", + license="Apache", + tests_require=["pytest"], + entry_points={ + "console_scripts": [ + "run = tutorial_skill_executor.run:main", + ], + }, +) diff --git a/tutorials/semantic_reasoning_demo/simulation_extras/tutorial_skill_executor/tutorial_skill_executor/__init__.py b/tutorials/semantic_reasoning_demo/simulation_extras/tutorial_skill_executor/tutorial_skill_executor/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/tutorials/semantic_reasoning_demo/simulation_extras/tutorial_skill_executor/tutorial_skill_executor/run.py b/tutorials/semantic_reasoning_demo/simulation_extras/tutorial_skill_executor/tutorial_skill_executor/run.py new file mode 100644 index 0000000..ba4ee4d --- /dev/null +++ b/tutorials/semantic_reasoning_demo/simulation_extras/tutorial_skill_executor/tutorial_skill_executor/run.py @@ -0,0 +1,480 @@ +import threading +import time +import random +import json +import rclpy +from rclpy.node import Node +from rclpy.action import ActionServer, ActionClient +from rclpy.callback_groups import MutuallyExclusiveCallbackGroup +from rclpy.executors import MultiThreadedExecutor +from rclpy.parameter import Parameter +from rclpy.action.server import ServerGoalHandle, GoalStatus +from overarching_msgs.action import Navigate, Detect, Pick, Place +from pyrobosim_msgs.msg import TaskAction, ExecutionResult +from pyrobosim_msgs.action import ExecuteTaskAction +from pyrobosim_msgs.srv import SetLocationState +from pyrobosim_msgs.srv import RequestWorldState +from std_srvs.srv import SetBool, Trigger +from std_msgs.msg import String, Float32 + +class SkillExecutor(Node): + def __init__(self): + super().__init__('tutorial_robot_action_server') + + # Initialize the boolean parameters + self._place_should_bump = False + self._navigation_should_fail = False + + # Initialize the last action performed + self._last_action = 'IDLE' + + # Initialize the pick status flag + self._pick_performed = False + + # Keep track of last executed place action + self._last_place_action = None + + # Initialize the last published force effort z value + self._last_force_effort_z = random.uniform(0.4, 0.6) + + # Create callback groups for the action/service servers and clients + self._servers_cb_group = MutuallyExclusiveCallbackGroup() + self._action_cb_group = MutuallyExclusiveCallbackGroup() + self._service_cb_group = MutuallyExclusiveCallbackGroup() + + # Create the /robot/exec_action_name publisher + self._exec_action_name_pub = self.create_publisher( + String, + '/robot/exec_action_name', + 10 + ) + + # Create the /robot/force_effort_z publisher + self._force_effort_z_pub = self.create_publisher( + Float32, + '/robot/force_effort_z', + 10 + ) + + # Publish 'IDLE' at initialization + self._publish_exec_action_name('IDLE') + + # Publish the initial random value on topic /robot/force_effort_z at initialization + self._publish_force_effort_z(self._last_force_effort_z) + + # Create a timer to regularly broadcast the last published value on topic /robot/force_effort_z + self._broadcast_timer = self.create_timer(1.0, self._broadcast_last_force_effort_z) + + # Create the /robot/executor/get_info service server + self._get_info_srv = self.create_service( + Trigger, + '/robot/executor/get_info', + self._get_info_callback, + callback_group=self._servers_cb_group + ) + + # Create the /robot/navigate action server + self._navigate_action_server = ActionServer( + self, + Navigate, + '/robot/navigate', + execute_callback=self._navigate_action_callback, + cancel_callback=self._cancel_callback, + callback_group=self._servers_cb_group + ) + + # Create the /robot/detect action server + self._detect_action_server = ActionServer( + self, + Detect, + '/robot/detect', + execute_callback=self._detect_action_callback, + cancel_callback=self._cancel_callback, + callback_group=self._servers_cb_group + ) + + # Create the /robot/pick action server + self._pick_action_server = ActionServer( + self, + Pick, + '/robot/pick', + execute_callback=self._pick_action_callback, + cancel_callback=self._cancel_callback, + callback_group=self._servers_cb_group + ) + + # Create the /robot/place action server + self._place_action_server = ActionServer( + self, + Place, + '/robot/place', + execute_callback=self._place_action_callback, + cancel_callback=self._cancel_callback, + callback_group=self._servers_cb_group + ) + + # Create the /robot/inject/spurious_navigation_bug service server + self._spurious_navigation_bug_srv = self.create_service( + SetBool, + '/robot/inject/spurious_navigation_bug', + self._spurious_navigation_bug_callback, + callback_group=self._servers_cb_group + ) + + # Create a client for the /set_location_state service + self._set_location_state_client = self.create_client( + SetLocationState, + '/set_location_state', + callback_group=self._service_cb_group + ) + + # Wait for the /set_location_state service to be available + while not self._set_location_state_client.wait_for_service(timeout_sec=2.0): + self.get_logger().info('Waiting for /set_location_state service...') + + # Create the /execute_action action client + self._execute_action_client = ActionClient( + self, + ExecuteTaskAction, + '/execute_action', + callback_group=self._action_cb_group + ) + + # Wait for the /execute_action action server to be available + while not self._execute_action_client.wait_for_server(timeout_sec=2.0): + self.get_logger().info('Waiting for /execute_action action server...') + + # Create the /request_world_state service client + self._world_state_client = self.create_client( + RequestWorldState, + '/request_world_state', + callback_group=self._service_cb_group + ) + + # Wait for the /request_world_state service server to be available + while not self._world_state_client.wait_for_service(timeout_sec=2.0): + self.get_logger().info('Waiting for /request_world_state service server...') + + def _publish_exec_action_name(self, action_name): + """Publish the current action name to the /robot/exec_action_name topic.""" + msg = String() + msg.data = action_name + self._exec_action_name_pub.publish(msg) + + # Update the last action performed + self._last_action = action_name + + def _publish_force_effort_z(self, value, duration_sec=0.0): + """Publish the force effort z value to the /robot/force_effort_z topic for the specified duration.""" + msg = Float32() + msg.data = value + start_time = time.time() + + if duration_sec > 0.0: + while time.time() - start_time < duration_sec: + self._force_effort_z_pub.publish(msg) + time.sleep(0.01) # Small sleep to prevent high CPU usage + else: + self._force_effort_z_pub.publish(msg) + + # Update the last published force effort z value + self._last_force_effort_z = value + + def _broadcast_last_force_effort_z(self): + """Broadcast the last published value on the /robot/force_effort_z topic.""" + # Generate a new random value based on the pick status + if self._pick_performed: + random_value = random.uniform(2.4, 2.6) + else: + random_value = random.uniform(0.4, 0.6) + + # Publish the new random value + self._publish_force_effort_z(random_value) + + def _execute_task_async(self, task_action): + """Send a task goal to the action server asynchronously.""" + if not self._execute_action_client.server_is_ready(): + # Wait for action server + if not self._execute_action_client.wait_for_server(timeout_sec=2.0): + self.get_logger().warn('Action server /execute_action not available (yet).') + else: + return None + + if not self._execute_action_client.server_is_ready(): + self.get_logger().error('Action server /execute_action is not ready') + return None + + goal_msg = ExecuteTaskAction.Goal() + goal_msg.action = task_action # task_action should be a TaskAction message + + self.get_logger().info(f'\nSending goal: {goal_msg}') + future = self._execute_action_client.send_goal_async(goal_msg) + return future + + def _execute_task_sync(self, task_action, cancel_event: threading.Event, timeout_sec=2.0): + """Send a task goal and wait for completion synchronously.""" + + goal_future = self._execute_task_async(task_action) + if goal_future is None: + return False, 'Action server not ready' + + # Wait for goal acceptance + start_time = time.time() + while not goal_future.done() and (time.time() - start_time) < timeout_sec: + time.sleep(0.01) + + if not goal_future.done(): + return False, 'Goal acceptance timeout' + + goal_handle: ClientGoalHandle = goal_future.result() + if not goal_handle.accepted: + return False, 'Goal was rejected' + + self.get_logger().info('Goal accepted, waiting for result...') + + # Wait for result + result_future = goal_handle.get_result_async() + start_time = time.time() + while not result_future.done() and not cancel_event.is_set(): + time.sleep(0.01) + + if result_future.done(): + result_response = result_future.result() + status = result_response.status + actual_result: ExecuteTaskAction.Result = result_response.result + success = status == GoalStatus.STATUS_SUCCEEDED and actual_result.execution_result.status == ExecutionResult.SUCCESS + return success, actual_result.execution_result.message + elif cancel_event.is_set(): + # self.get_logger().info(f"Cancelling action {task_action}") + goal_handle.cancel_goal() + return False, 'Cancelled' + else: + return False, 'Result timeout' + + def _process_execute_task_action(self, goal_handle: ServerGoalHandle, task_action: TaskAction, result): + # if self._curr_robot_status and self._curr_robot_status.executing_action: + # time.sleep(0.5) + + self._cancel_event = threading.Event() + self._processed_task_sync_event = threading.Event() + success, message = self._execute_task_sync(task_action, cancel_event=self._cancel_event) + self.get_logger().debug(f"Task action execution completed: success={success}, message='{message}'") + self._processed_task_sync_event.set() + + if goal_handle.is_active: + goal_handle.succeed() if success else goal_handle.abort() + + # Publish 'IDLE' once again as soon as the action server callback is exited + self._publish_exec_action_name('IDLE') + + return result + + def _get_world_state(self): + req = RequestWorldState.Request() + future = self._world_state_client.call_async(req) + + done_event = threading.Event() + + def cb(_): + done_event.set() + + future.add_done_callback(cb) + done_event.wait() + + return future.result() + + def _cancel_callback(self, goal_handle: ServerGoalHandle): + if goal_handle.is_active: + self._cancel_event.set() + self.get_logger().warn(f"Requested to cancel active action with goal id {goal_handle.goal_id} with request {goal_handle.request}") + self._processed_task_sync_event.wait() + self.get_logger().warn(f"Cancelled active action with goal id {goal_handle.goal_id} with request {goal_handle.request}") + return CancelResponse.ACCEPT + return CancelResponse.REJECT + + def _get_info_callback(self, request, response): + """Serialize the info regarding the last executed place action.""" + info = [] + + if self._last_place_action is not None: + place_info = { + "dt_id": f"place{self._last_place_action['id']}", + "place_def": { + "param": { + "who" : "robot", + "what" : self._last_place_action['object_id'], + "where": self._last_place_action['location_id'] + }, + "result": "success" if self._last_place_action['success'] else "failure" + } + } + info.append(place_info) + + response.message = json.dumps(info, indent=2) + response.success = True + + return response + + def _spurious_navigation_bug_callback(self, request, response): + # Set the place_should_bump and navigation_should_fail parameters to true + self._place_should_bump = request.data + self._navigation_should_fail = request.data + + action = "injected" if request.data else "removed" + + # Set the response + response.success = True + response.message = f"Successfully {action} spurious navigation bug" + + return response + + def _navigate_action_callback(self, goal_handle: ServerGoalHandle): + request: Navigate.Goal = goal_handle.request + + # Publish the current action name + self._publish_exec_action_name('NAVIGATE') + + # Create TaskAction message properly + task_action = TaskAction() + task_action.robot = 'robot' + task_action.type = 'navigate' + task_action.target_location = request.location_id if not self._navigation_should_fail else request.location_id + '_left' + + return self._process_execute_task_action(goal_handle, task_action, Navigate.Result()) + + def _detect_action_callback(self, goal_handle): + request: Detect.Goal = goal_handle.request + + # Publish the current action name + self._publish_exec_action_name('DETECT') + + # Create TaskAction message properly + task_action = TaskAction() + task_action.robot = 'robot' + task_action.type = 'detect' + + return self._process_execute_task_action(goal_handle, task_action, Detect.Result()) + + def _pick_action_callback(self, goal_handle): + request: Pick.Goal = goal_handle.request + + # Publish the current action name + self._publish_exec_action_name('PICK') + + mapping = { + 0: "soda0", + 1: "butter0", + 2: "snacks0" + } + + obj = mapping.get(goal_handle.request.object_id) + + if obj is None: + self.get_logger().error( + f"Invalid object_id: {goal_handle.request.object_id}" + ) + goal_handle.abort() + return result + + # Create TaskAction message properly + task_action = TaskAction() + task_action.robot = 'robot' + task_action.type = 'pick' + task_action.object = obj + + result = self._process_execute_task_action(goal_handle, task_action, Pick.Result()) + + # Set the pick status flag to True + self._pick_performed = True + + # Generate a random value between 2.4 and 2.6 and publish it on topic /robot/force_effort_z + random_value = random.uniform(2.4, 2.6) + self._publish_force_effort_z(random_value) + + return result + + def _place_action_callback(self, goal_handle): + request: Place.Goal = goal_handle.request + result: Place.Result = Place.Result() + + # Publish the current action name + self._publish_exec_action_name('PLACE') + + # Get world state + world_state = self._get_world_state() + if world_state is None: + goal_handle.abort() + return result + robot_state = world_state.state.robots[0] + object_id = robot_state.manipulated_object + location_id = 'fridge' if 'fridge' in robot_state.last_visited_location else robot_state.last_visited_location + + succ = True + if self._place_should_bump: + # Cancel the broadcast timer + self._broadcast_timer.cancel() + + # Publish a random value between 4.9 and 5.1 on topic /robot/force_effort_z for a duration of 300ms + random_value_1 = random.uniform(4.9, 5.1) + self._publish_force_effort_z(random_value_1, 0.3) + + # Publish a random value between 2.4 and 2.6 on topic /robot/force_effort_z for a duration of 100ms + random_value_2 = random.uniform(2.4, 2.6) + self._publish_force_effort_z(random_value_2, 0.1) + + # Restart the broadcast timer + self._broadcast_timer = self.create_timer(1.0, self._broadcast_last_force_effort_z) + + goal_handle.abort() + succ = False + else: + # Create TaskAction message properly + task_action = TaskAction() + task_action.robot = 'robot' + task_action.type = 'place' + + result = self._process_execute_task_action(goal_handle, task_action, Place.Result()) + + # Set the pick status flag to False + self._pick_performed = False + + # Generate a random value between 0.4 and 0.6 and publish it on topic /robot/force_effort_z + random_value = random.uniform(0.4, 0.6) + self._publish_force_effort_z(random_value) + + # Store the last executed place action + self._last_place_action = { + 'id': '44', + 'object_id': object_id, + 'location_id': location_id, + 'success': succ + } + + self.get_logger().info( + f"Place --> object={object_id}, location={location_id}, success={succ}" + ) + + return result + +def main(args=None): + rclpy.init(args=args) + + # Create an instance of the SkillExecutor node + node = SkillExecutor() + + # Use a MultiThreadedExecutor to handle the reentrant callback group + executor = MultiThreadedExecutor() + executor.add_node(node) + + try: + # Spin the node using the executor + executor.spin() + except KeyboardInterrupt: + pass + finally: + # Clean up + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/tutorials/semantic_reasoning_demo/simulation_extras/tutorial_skill_executor/worlds/location_data.yaml b/tutorials/semantic_reasoning_demo/simulation_extras/tutorial_skill_executor/worlds/location_data.yaml new file mode 100644 index 0000000..d5ca2dd --- /dev/null +++ b/tutorials/semantic_reasoning_demo/simulation_extras/tutorial_skill_executor/worlds/location_data.yaml @@ -0,0 +1,180 @@ +##################### +# Location metadata # +##################### + +table: + footprint: + type: box + dims: [1.2, 0.8] + height: 0.5 + nav_poses: + - position: # above + x: 0.0 + y: 0.65 + rotation_eul: + yaw: -1.57 + - position: # below + x: 0.0 + y: -0.65 + rotation_eul: + yaw: 1.57 + - position: # left + x: -0.85 + y: 0.0 + - position: # right + x: 0.85 + y: 0.0 + rotation_eul: + yaw: 3.14 + locations: + - name: "tabletop" + footprint: + type: parent + padding: 0.05 + color: [0.2, 0.2, 0.2] + +desk: + footprint: + type: box + dims: [0.6, 1.2] + height: 0.5 + nav_poses: + - position: # left + x: -0.5 + y: 0.0 + - position: # right + x: 0.5 + y: 0.0 + rotation_eul: + yaw: 3.14 + 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: + - position: # above + x: 0.0 + y: 0.55 + rotation_eul: + yaw: -1.57 + - position: # below + x: 0.0 + y: -0.55 + rotation_eul: + yaw: 1.57 + locations: + - name: "storage" + footprint: + type: parent + padding: 0.05 + - name: "left" # The location name will be "_left" + footprint: + type: parent + padding: 0.05 + nav_poses: + - [-0.52, 0.5, 0.0, -1.57] + color: [0.2, 0.5, 0.2] + +trashcan_small: + footprint: + type: circle + radius: 0.25 + height: 0.5 + nav_poses: + - position: # above + x: 0.0 + y: 0.5 + rotation_eul: + yaw: -1.57 + - position: # below + x: 0.0 + y: -0.5 + rotation_eul: + yaw: 1.57 + - position: # left + x: -0.5 + y: 0.0 + - position: # right + x: 0.5 + y: 0.0 + rotation_eul: + yaw: 3.14 + 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: + - position: # above + x: 0.0 + y: 0.75 + rotation_eul: + yaw: -1.57 + - position: # below + x: 0.0 + y: -0.75 + rotation_eul: + yaw: 1.57 + - position: # left + x: -0.75 + y: 0.0 + - position: # right + x: 0.75 + y: 0.0 + rotation_eul: + yaw: 3.14 + 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: + - position: # above + x: 0.0 + y: 0.4 + rotation_eul: + yaw: -1.57 + - position: # below + x: 0.0 + y: -0.4 + rotation_eul: + yaw: 1.57 + - position: # left + x: -0.7 + y: 0.0 + - position: # right + x: 0.7 + y: 0.0 + rotation_eul: + yaw: 3.14 + color: [0.4, 0.4, 0] diff --git a/tutorials/semantic_reasoning_demo/simulation_extras/tutorial_skill_executor/worlds/object_data.yaml b/tutorials/semantic_reasoning_demo/simulation_extras/tutorial_skill_executor/worlds/object_data.yaml new file mode 100644 index 0000000..fc6332a --- /dev/null +++ b/tutorials/semantic_reasoning_demo/simulation_extras/tutorial_skill_executor/worlds/object_data.yaml @@ -0,0 +1,33 @@ +################### +# Object metadata # +################### + +bread: + footprint: + type: box + 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/tutorials/semantic_reasoning_demo/simulation_extras/tutorial_skill_executor/worlds/old_world.yaml b/tutorials/semantic_reasoning_demo/simulation_extras/tutorial_skill_executor/worlds/old_world.yaml new file mode 100644 index 0000000..8c17f46 --- /dev/null +++ b/tutorials/semantic_reasoning_demo/simulation_extras/tutorial_skill_executor/worlds/old_world.yaml @@ -0,0 +1,297 @@ +# Problem 4 World +# Adds battery usage to actions. + +metadata: + locations: $PWD/location_data.yaml + objects: $PWD/object_data.yaml + +params: + name: convince_world + inflation_radius: 0.01 + object_radius: 0.01 + +robots: + - name: robot + radius: 0.1 + location: dining + partial_obs_objects: true + partial_obs_hallways: true + sensors: + lidar: + type: lidar + update_rate_s: 0.1 + angle_units: degrees + min_angle: -120.0 + max_angle: 120.0 + angular_resolution: 5.0 + max_range_m: 2.0 + path_executor: + type: constant_velocity + lidar_sensor_name: lidar + path_planner: + type: rrt + bidirectional: true + max_connection_dist: 0.5 + max_time: 10.0 + action_execution_options: + navigate: + success_probability: 0.9 + rng_seed: 42 + 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: 8 + close: + success_probability: 0.75 + battery_usage: 8 + detect: + success_probability: 0.8 + battery_usage: 0 + +rooms: + - name: dining + footprint: + type: polygon + coords: + - [-1.6, -1.0] + - [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: + - position: # left + x: -1.0 + y: 0.0 + - position: # right + x: 1.0 + y: 0.0 + rotation_eul: + yaw: 3.14 + - position: # above + x: 0.0 + y: 0.8 + rotation_eul: + yaw: -1.57 + - position: # below + x: 0.0 + y: -0.8 + rotation_eul: + yaw: 1.57 + 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: trash + footprint: + type: polygon + coords: + - [-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.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: 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 + parent: dining + category: table + pose: + position: + x: 0.0 + y: 0.15 + + - name: desk + parent: office + category: desk + pose: + position: + x: 3.6 + y: -2.3 + + - name: fridge + parent: kitchen + category: storage + pose: + position: + x: -3.0 + y: -2.65 + is_open: false + + - name: pantry + parent: kitchen + category: storage + pose: + position: + x: -3.65 + y: -1.5 + rotation_eul: + yaw: 1.57 + is_open: false + + - name: bin + parent: office + category: trashcan_small + pose: + position: + x: 2.6 + y: -0.85 + is_open: true + + - name: dumpster + parent: trash + category: trashcan_large + pose: + position: + x: -3.4 + y: 2.4 + is_open: false + + - name: charger + parent: closet + category: charger + pose: + position: + x: 3.5 + y: 2.5 + rotation_eul: + yaw: -0.785 + is_charger: true + +objects: + - 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/tutorials/semantic_reasoning_demo/simulation_extras/tutorial_skill_executor/worlds/world.yaml b/tutorials/semantic_reasoning_demo/simulation_extras/tutorial_skill_executor/worlds/world.yaml new file mode 100644 index 0000000..a850092 --- /dev/null +++ b/tutorials/semantic_reasoning_demo/simulation_extras/tutorial_skill_executor/worlds/world.yaml @@ -0,0 +1,151 @@ +# Problem 4 World +# Adds battery usage to actions. + +metadata: + locations: $PWD/location_data.yaml + objects: $PWD/object_data.yaml + +params: + name: convince_world + inflation_radius: 0.01 + object_radius: 0.01 + +robots: + - name: robot + radius: 0.1 + color: "#000" + location: hall + partial_obs_objects: false + path_executor: + type: constant_velocity + path_planner: + type: rrt + bidirectional: false + max_connection_dist: 0.5 + max_time: 10.0 + action_execution_options: + navigate: + success_probability: 1.0 + rng_seed: 42 + # battery_usage: 0.5 # Per meter traveled + pick: + success_probability: 1.0 + # battery_usage: 5 + place: + success_probability: 1.0 + # battery_usage: 5 + open: + success_probability: 1.0 + # battery_usage: 8 + close: + success_probability: 1.0 + # battery_usage: 8 + detect: + success_probability: 1.0 + # battery_usage: 0 + +rooms: + - name: dining + footprint: + type: polygon + coords: + - [-5, 5] + - [-5, 2] + - [-2, 2] + - [-2, 5] + wall_width: 0.3 + color: "#FA9F00" + + - name: kitchen + footprint: + type: polygon + coords: + - [-5, -5] + - [-5, -2] + - [-2, -2] + - [-2, -5] + wall_width: 0.3 + color: "#009A95" + + - name: hall + footprint: + type: polygon + coords: + - [5, -5] + - [5, -2] + - [2, -2] + - [2, -5] + wall_width: 0.3 + color: "#F70063" + +hallways: + - room_start: dining + room_end: kitchen + width: 0.8 + wall_width: 0.05 + color: "#CCC" + conn_method: angle + conn_angle: -1.57 + offset: 0 + is_open: true + is_locked: false + + - room_start: kitchen + room_end: hall + width: 0.8 + wall_width: 0.05 + color: "#CCC" + conn_method: angle + conn_angle: 0 + offset: 0 + is_open: faltruese + is_locked: false + + - room_start: dining + room_end: hall + width: 0.8 + wall_width: 0.05 + color: "#CCC" + conn_method: angle + conn_angle: -0.77 + offset: .9 + is_open: true + is_locked: false + +locations: + - name: dining_table + parent: dining + category: table + pose: + position: + x: -2.75 + y: 4.5 + - name: side_table + parent: dining + category: table + pose: + position: + x: -4.25 + y: 4.5 + - name: kitchen_table + parent: kitchen + category: table + pose: + position: + x: -4.25 + y: -4.5 + - name: fridge + parent: kitchen + category: storage + pose: + position: + x: -2.75 + y: -4.5 + +objects: + - parent: kitchen_table + category: soda + - parent: side_table + category: snacks + - parent: fridge + category: butter