diff --git a/.github/workflows/main.yaml b/.github/workflows/main.yaml index c9550be..a7657f9 100644 --- a/.github/workflows/main.yaml +++ b/.github/workflows/main.yaml @@ -15,16 +15,34 @@ env: jobs: build-and-test: - runs-on: ${{ matrix.os }} - container: - image: osrf/ros:humble-desktop - strategy: - matrix: - os: [ubuntu-22.04] - fail-fast: false + runs-on: ubuntu-22.04 steps: - name: Checkout repository uses: actions/checkout@v4 + - name: Set up ROS 2 sources and key + run: | + sudo apt-get update + sudo apt-get install -y curl gnupg lsb-release + sudo curl -fsSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key | \ + gpg --dearmor | sudo tee /usr/share/keyrings/ros-archive-keyring.gpg > /dev/null + echo "deb [arch=amd64 signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(lsb_release -cs) main" | \ + sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null + sudo apt-get update + - name: Install missing dependencies + run: | + sudo apt-get update + sudo apt-get install -y libunwind-dev + - name: Install ROS 2 Humble and tools + run: | + sudo apt install -y ros-humble-desktop \ + ros-humble-behaviortree-cpp-v3 \ + ros-humble-test-msgs \ + ros-humble-qt-gui-cpp \ + ros-humble-rqt-gui-cpp \ + ros-humble-navigation2 \ + ros-humble-nav2-bringup \ + python3-colcon-common-extensions \ + python3-vcstool python3-pytest python3-colcon-coveragepy-result libfl-dev - name: Install deps run: sudo apt-get update && sudo apt-get install -y wget python3-vcstool python3-pytest python3-colcon-coveragepy-result libfl-dev pip && pip install ConfigSpace && pip install typing_extensions==4.7.1 --upgrade - name: fix pytest name @@ -49,8 +67,13 @@ jobs: pip install unified_planning[tamer] - name: Create custom repos run: cp upf.repos /tmp/all.repos + - name: Initialize rosdep + run: | + sudo apt install -y python3-rosdep + sudo rosdep init || true + rosdep update - name: build and test - uses: ros-tooling/action-ros-ci@0.3.15 + uses: ros-tooling/action-ros-ci@0.2.5 with: package-name: upf4ros2 upf_msgs target-ros2-distro: humble @@ -58,12 +81,10 @@ jobs: colcon-defaults: | { "test": { - "parallel-workers" : 4 + "parallel-workers" : 1 } } - colcon-mixin-name: coverage-gcc colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/main/index.yaml - extra-args: "--event-handlers console_direct+ --return-code-on-test-failure" - name: Codecov uses: codecov/codecov-action@v1.2.1 with: diff --git a/Dockerfile b/Dockerfile new file mode 100644 index 0000000..125e927 --- /dev/null +++ b/Dockerfile @@ -0,0 +1,74 @@ +FROM ubuntu:jammy + +ENV DEBIAN_FRONTEND=noninteractive +ENV ROS_DISTRO=humble + +# 1. Dependencias básicas +RUN apt-get update && apt-get install -y \ + curl \ + gnupg2 \ + lsb-release \ + locales \ + software-properties-common \ + python3-pip \ + git \ + libreadline-dev \ + openjdk-17-jdk \ + openjdk-17-jre \ + wget \ + ca-certificates && \ + locale-gen en_US en_US.UTF-8 && \ + update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8 + +# 2. Clave GPG y repositorio de ROS 2 +RUN curl -fsSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key | \ + gpg --dearmor -o /usr/share/keyrings/ros-archive-keyring.gpg && \ + echo "deb [arch=amd64 signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(lsb_release -cs) main" \ + > /etc/apt/sources.list.d/ros2.list && \ + apt-get update + +# 3. Instalar ROS 2 Humble +RUN apt-get install -y ros-humble-desktop ros-humble-behaviortree-cpp-v3 ros-humble-test-msgs ros-humble-qt-gui-cpp ros-humble-rqt-gui-cpp ros-humble-navigation2 ros-humble-nav2-bringup + +# 4. Instalar herramientas de desarrollo ROS 2 +RUN apt-get install -y \ + build-essential \ + cmake \ + python3-colcon-common-extensions \ + python3-vcstool \ + python3-rosdep \ + python3-argcomplete + +# 5. Inicializar rosdep +RUN rosdep init && rosdep update + +# 6. Instalar Python tools para UPF4ROS2 +RUN pip install --upgrade pip && \ + pip install \ + setuptools \ + typing_extensions==4.7.1 \ + ConfigSpace && \ + pip install --pre 'unified-planning[pyperplan,tamer]' + +RUN pip install --upgrade pip scipy + + +# 7. Crear workspace y clonar UPF4ROS2 +WORKDIR /workspace +RUN mkdir -p src && cd src && \ + git clone https://github.com/PlanSys2/UPF4ROS2.git && \ + vcs import . < UPF4ROS2/upf.repos + +# 8. Resolver dependencias con rosdep +RUN . /opt/ros/${ROS_DISTRO}/setup.sh && \ + rosdep install --from-paths src --ignore-src -r -y + +# 9. Compilar +RUN . /opt/ros/${ROS_DISTRO}/setup.sh && \ + colcon build --symlink-install + +# 10. Configurar entorno +RUN echo "source /opt/ros/${ROS_DISTRO}/setup.bash" >> ~/.bashrc && \ + echo "source /workspace/install/setup.bash" >> ~/.bashrc + +CMD ["bash"] diff --git a/LICENSE.txt b/LICENSE.txt new file mode 100644 index 0000000..261eeb9 --- /dev/null +++ b/LICENSE.txt @@ -0,0 +1,201 @@ + 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/README.md b/README.md index a7af3e1..0217c78 100644 --- a/README.md +++ b/README.md @@ -1,11 +1,24 @@ # UPF4ROS2 -[![main](https://github.com/igonzf/UPF4ROS2/actions/workflows/main.yaml/badge.svg)](https://github.com/igonzf/UPF4ROS2/actions/workflows/main.yaml) +[![main](https://github.com/PlanSys2/UPF4ROS2/actions/workflows/main.yaml/badge.svg)](https://github.com/PlanSys2/UPF4ROS2/actions/workflows/main.yaml) This repository integrates the Unified Planning Framework (UPF) into the ROS 2 ecosystem, providing a modular and standardized solution for automated planning in robotic systems. This project is part of the European initiative AIPlan4EU, which aims to develop automated planning tools that are accessible and applicable across different engineering domains. ## Install and building +``` +sudo apt update +sudo apt install \ + ros-humble-behaviortree-cpp-v3 \ + ros-humble-test-msgs \ + ros-humble-qt-gui-cpp \ + ros-humble-rqt-gui-cpp \ + ros-humble-navigation2 \ + ros-humble-nav2-bringup \ + python3-pip python3-colcon-common-extensions + +``` + ``` $ pip install --pre unified-planning[pyperplan,tamer] $ pip install ConfigSpace @@ -58,6 +71,23 @@ $ cd unified-planning $ bash run_tests.sh ``` +## Docker + +A `Dockerfile` is provided in the repository to build a containerized environment for UPF4ROS2. This can be useful to ensure all dependencies are correctly installed and the development environment is reproducible. + +To build the Docker image: + +``` +$ cd /src/UPF4ROS2 +$ docker build -t upf4ros2 . +``` + +To run the container: + +``` +docker run -it upf4ros2 +``` + ## Usage `$ ros2 launch upf4ros2 upf4ros2.launch.py` @@ -70,9 +100,9 @@ $ bash run_tests.sh - `/upf4ros2/add_fluent` `[upf_msgs/srv/AddFluent]` - `/upf4ros2/add_goal` `[upf_msgs/srv/AddGoal]` - `/upf4ros2/add_object` `[upf_msgs/srv/AddObject]` - - `/upf4ros2/new_problem` [upf_msgs/srv/NewProblem]` - - `/upf4ros2/set_initial_value` [upf_msgs/srv/SetInitialValue]` - - `/upf4ros2/set_problem` [upf_msgs/srv/SetProblem]` + - `/upf4ros2/new_problem` `[upf_msgs/srv/NewProblem]` + - `/upf4ros2/set_initial_value` `[upf_msgs/srv/SetInitialValue]` + - `/upf4ros2/set_problem` `[upf_msgs/srv/SetProblem]` - Actions: - `/upf4ros2/planOneShotPDDL` `[upf_msgs/action/PDDLPlanOneShot]` - `/upf4ros2/planOneShot` `[upf_msgs/action/PlanOneShot]` @@ -130,6 +160,30 @@ There are two alternatives: - Replanning case: one of the waypoints is not reachable and it is necessary to replan. Illustrated in this [video](https://youtu.be/UJncg7GPCro) +### [Demo 3 - Harvest](https://youtu.be/21oZHhl7kcA) + +This demo consists of creating the problem from a ROS 2 node to navigate a crop field to perform a harvest collection mision. +For run this demo I used the simulated Leo Rover robot in Gazebo Ignition Fortress from this [repo](https://github.com/luispri2001/gps_ignition_simulation) + +`$ ros2 launch upf4ros2 upf4ros2.launch.py` + +`$ ros2 launch upf4ros2_demo upf4ros2_demo3_harvest.launch.py` + +## Metrics + +| demo | planner | mean_time_s | median_time_s | std_time_s | min_s | max_s | num_runs | +| ---------------- | -------- | ----------- | ------------- | ---------- | ------ | ------ | -------- | +| Demo 1 - PDDL | UPF | 0.06891 | 0.0687 | 0.00078 | 0.0678 | 0.0706 | 10 | +| Demo 1 - PDDL | UPF4ROS2 | 0.08873 | 0.0667 | 0.03617 | 0.0653 | 0.1460 | 10 | +| Demo 1 | UPF | 0.04425 | 0.0442 | 0.00093 | 0.0433 | 0.0467 | 10 | +| Demo 1 | UPF4ROS2 | 0.1766 | 0.1817 | 0.02737 | 0.1009 | 0.1949 | 10 | +| Demo 2 | UPF | 0.04439 | 0.0444 | 0.00035 | 0.0438 | 0.0448 | 10 | +| Demo 2 | UPF4ROS2 | 0.18429 | 0.1816 | 0.00868 | 0.1771 | 0.2035 | 10 | +| Demo 3 | UPF | 0.04703 | 0.0466 | 0.0013 | 0.0454 | 0.0498 | 10 | +| Demo 3 | UPF4ROS2 | 0.22318 | 0.2258 | 0.02218 | 0.1767 | 0.2592 | 10 | +| Demo 3 - Harvest | UPF | 0.18 | 0.04965 | 0.0048 | 0.0482 | 0.064 | 10 | +| Demo 3 - Harvest | UPF4ROS2 | 0.30883 | 0.31305 | 0.05294 | 0.1778 | 0.3678 | 10 | + ## Acknowledgments diff --git a/data/metrics.csv b/data/metrics.csv new file mode 100644 index 0000000..9eb0873 --- /dev/null +++ b/data/metrics.csv @@ -0,0 +1,11 @@ +demo,planner,mean_time_s,median_time_s,std_time_s,min_s, max_s,num_runs, +Demo 1 - PDDL,UPF,0.06891,0.0687,0.00078,0.0678,0.0706,10 +Demo 1 - PDDL,UPF4ROS2,0.08873,0.0667,0.03617,0.0653,0.1460,10 +Demo 1,UPF,0.04425,0.0442,0.00093,0.0433,0.0467,10 +Demo 1,UPF4ROS2,0.1766,0.1817,0.02737,0.1009,0.1949,10 +Demo 2,UPF,0.04439,0.0444,0.00035,0.0438,0.0448,10 +Demo 2,UPF4ROS2,0.18429,0.1816,0.00868,0.1771,0.2035,10 +Demo 3,UPF,0.04703,0.0466,0.0013,0.0454,0.0498,10 +Demo 3,UPF4ROS2,0.22318,0.2258,0.02218,0.1767,0.2592,10 +Demo 3 - Harvest,UPF,0.18,0.04965,0.0048,0.0482,0.064,10 +Demo 3 - Harvest,UPF4ROS2,0.30883,0.31305,0.05294,0.1778,0.3678,10 diff --git a/data/metrics.md b/data/metrics.md new file mode 100644 index 0000000..d43a2a7 --- /dev/null +++ b/data/metrics.md @@ -0,0 +1,12 @@ +demo | planner | mean_time_s | median_time_s | std_time_s | min_s | max_s | num_runs | +------------------|------------|---------------|-----------------|--------------|----------|----------|------------|--- +Demo 1 - PDDL | UPF | 0.06891 | 0.0687 | 0.00078 | 0.0678 | 0.0706 | 10 | +Demo 1 - PDDL | UPF4ROS2 | 0.08873 | 0.0667 | 0.03617 | 0.0653 | 0.1460 | 10 | +Demo 1 | UPF | 0.04425 | 0.0442 | 0.00093 | 0.0433 | 0.0467 | 10 | +Demo 1 | UPF4ROS2 | 0.1766 | 0.1817 | 0.02737 | 0.1009 | 0.1949 | 10 | +Demo 2 | UPF | 0.04439 | 0.0444 | 0.00035 | 0.0438 | 0.0448 | 10 | +Demo 2 | UPF4ROS2 | 0.18429 | 0.1816 | 0.00868 | 0.1771 | 0.2035 | 10 | +Demo 3 | UPF | 0.04703 | 0.0466 | 0.0013 | 0.0454 | 0.0498 | 10 | +Demo 3 | UPF4ROS2 | 0.22318 | 0.2258 | 0.02218 | 0.1767 | 0.2592 | 10 | +Demo 3 - Harvest | UPF | 0.18 | 0.04965 | 0.0048 | 0.0482 | 0.064 | 10 | +Demo 3 - Harvest | UPF4ROS2 | 0.30883 | 0.31305 | 0.05294 | 0.1778 | 0.3678 | 10 | diff --git a/data/raw_results.csv b/data/raw_results.csv new file mode 100644 index 0000000..7c1dfdd --- /dev/null +++ b/data/raw_results.csv @@ -0,0 +1,101 @@ +demo,run,planner,time_s +Demo 1 - PDDL,1,UPF,0.0694 +Demo 1 - PDDL,1,UPF4ROS2,0.146 +Demo 1 - PDDL,2,UPF,0.0686 +Demo 1 - PDDL,2,UPF4ROS2,0.1362 +Demo 1 - PDDL,3,UPF,0.0687 +Demo 1 - PDDL,3,UPF4ROS2,0.0659 +Demo 1 - PDDL,4,UPF,0.0706 +Demo 1 - PDDL,4,UPF4ROS2,0.0653 +Demo 1 - PDDL,5,UPF,0.0695 +Demo 1 - PDDL,5,UPF4ROS2,0.0663 +Demo 1 - PDDL,6,UPF,0.0685 +Demo 1 - PDDL,6,UPF4ROS2,0.1409 +Demo 1 - PDDL,7,UPF,0.0685 +Demo 1 - PDDL,7,UPF4ROS2,0.0675 +Demo 1 - PDDL,8,UPF,0.0691 +Demo 1 - PDDL,8,UPF4ROS2,0.0671 +Demo 1 - PDDL,9,UPF,0.0678 +Demo 1 - PDDL,9,UPF4ROS2,0.0661 +Demo 1 - PDDL,10,UPF,0.0684 +Demo 1 - PDDL,10,UPF4ROS2,0.066 +Demo 1,1,UPF,0.0442 +Demo 1,1,UPF4ROS2,0.1784 +Demo 1,2,UPF,0.044 +Demo 1,2,UPF4ROS2,0.1009 +Demo 1,3,UPF,0.0435 +Demo 1,3,UPF4ROS2,0.182 +Demo 1,4,UPF,0.0443 +Demo 1,4,UPF4ROS2,0.1808 +Demo 1,5,UPF,0.0442 +Demo 1,5,UPF4ROS2,0.1754 +Demo 1,6,UPF,0.0437 +Demo 1,6,UPF4ROS2,0.1813 +Demo 1,7,UPF,0.0444 +Demo 1,7,UPF4ROS2,0.1913 +Demo 1,8,UPF,0.0442 +Demo 1,8,UPF4ROS2,0.1949 +Demo 1,9,UPF,0.0467 +Demo 1,9,UPF4ROS2,0.1903 +Demo 1,10,UPF,0.0433 +Demo 1,10,UPF4ROS2,0.1907 +Demo 2,1,UPF,0.0444 +Demo 2,1,UPF4ROS2,0.1863 +Demo 2,2,UPF,0.0443 +Demo 2,2,UPF4ROS2,0.1782 +Demo 2,3,UPF,0.0438 +Demo 2,3,UPF4ROS2,0.1775 +Demo 2,4,UPF,0.0448 +Demo 2,4,UPF4ROS2,0.1838 +Demo 2,5,UPF,0.0439 +Demo 2,5,UPF4ROS2,0.1944 +Demo 2,6,UPF,0.0444 +Demo 2,6,UPF4ROS2,0.1771 +Demo 2,7,UPF,0.0447 +Demo 2,7,UPF4ROS2,0.1793 +Demo 2,8,UPF,0.0442 +Demo 2,8,UPF4ROS2,0.1778 +Demo 2,9,UPF,0.0447 +Demo 2,9,UPF4ROS2,0.2035 +Demo 2,10,UPF,0.0447 +Demo 2,10,UPF4ROS2,0.185 +Demo 3,1,UPF,0.0454 +Demo 3,1,UPF4ROS2,0.1767 +Demo 3,2,UPF,0.0467 +Demo 3,2,UPF4ROS2,0.2592 +Demo 3,3,UPF,0.0465 +Demo 3,3,UPF4ROS2,0.2365 +Demo 3,4,UPF,0.0498 +Demo 3,4,UPF4ROS2,0.2235 +Demo 3,5,UPF,0.0464 +Demo 3,5,UPF4ROS2,0.2193 +Demo 3,6,UPF,0.0478 +Demo 3,6,UPF4ROS2,0.228 +Demo 3,7,UPF,0.0485 +Demo 3,7,UPF4ROS2,0.2003 +Demo 3,8,UPF,0.0465 +Demo 3,8,UPF4ROS2,0.2213 +Demo 3,9,UPF,0.0461 +Demo 3,9,UPF4ROS2,0.2363 +Demo 3,10,UPF,0.0466 +Demo 3,10,UPF4ROS2,0.2307 +Demo 3 - Harvest,1,UPF,0.064 +Demo 3 - Harvest,1,UPF4ROS2,0.1778 +Demo 3 - Harvest,2,UPF,0.0491 +Demo 3 - Harvest,2,UPF4ROS2,0.3216 +Demo 3 - Harvest,3,UPF,0.0486 +Demo 3 - Harvest,3,UPF4ROS2,0.3397 +Demo 3 - Harvest,4,UPF,0.0499 +Demo 3 - Harvest,4,UPF4ROS2,0.3008 +Demo 3 - Harvest,5,UPF,0.0506 +Demo 3 - Harvest,5,UPF4ROS2,0.3045 +Demo 3 - Harvest,6,UPF,0.0482 +Demo 3 - Harvest,6,UPF4ROS2,0.283 +Demo 3 - Harvest,7,UPF,0.0486 +Demo 3 - Harvest,7,UPF4ROS2,0.3028 +Demo 3 - Harvest,8,UPF,0.0494 +Demo 3 - Harvest,8,UPF4ROS2,0.3408 +Demo 3 - Harvest,9,UPF,0.0502 +Demo 3 - Harvest,9,UPF4ROS2,0.3495 +Demo 3 - Harvest,10,UPF,0.054 +Demo 3 - Harvest,10,UPF4ROS2,0.3678 diff --git a/plansys2_upf_plan_solver/CMakeLists.txt b/plansys2_upf_plan_solver/CMakeLists.txt index 5b5f5ec..0492ae5 100644 --- a/plansys2_upf_plan_solver/CMakeLists.txt +++ b/plansys2_upf_plan_solver/CMakeLists.txt @@ -42,6 +42,8 @@ install(FILES plansys2_upf_plan_solver_plugin.xml DESTINATION share/${PROJECT_NAME} ) + + install(TARGETS ${PROJECT_NAME} ARCHIVE DESTINATION lib diff --git a/upf.repos b/upf.repos index 14a7eae..0ab964c 100644 --- a/upf.repos +++ b/upf.repos @@ -1,4 +1,8 @@ repositories: + simple_node: + type: git + url: https://github.com/uleroboticsgroup/simple_node.git + version: main unified-planning: type: git url: https://github.com/aiplan4eu/unified-planning.git diff --git a/upf4ros2_demo/launch/upf4ros2_demo3_harvest.launch.py b/upf4ros2_demo/launch/upf4ros2_demo3_harvest.launch.py new file mode 100644 index 0000000..6c77495 --- /dev/null +++ b/upf4ros2_demo/launch/upf4ros2_demo3_harvest.launch.py @@ -0,0 +1,63 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + + pkg_name = "upf4ros2_demo" + + # + # ARGS + # + + points = LaunchConfiguration("wps") + declare_points_cmd = DeclareLaunchArgument( + "wps", + default_value=get_package_share_directory( + pkg_name) + "/params/house.yaml", + description="YAML waypoints file") + + # + # ACTIONS + # + + upf4ros2_navigation_action_cmd = Node( + package=pkg_name, + executable="upf4ros2_navigation_action", + name="upf4ros2_navigation_action", + parameters=[points], + output='screen') + + upf4ros2_check_wp_action_cmd = Node( + package=pkg_name, + executable="upf4ros2_check_wp_action", + name="upf4ros2_check_wp_action", + output='screen') + + upf4ros2_collect_action_cmd = Node( + package=pkg_name, + executable="upf4ros2_collect_action", + name="upf4ros2_collect_action", + output='screen') + + # + # NODES + # + + upf4ros2_demo_cmd = Node( + package=pkg_name, + executable="upf4ros2_demo3_harvest", + name="upf4ros2_demo3_harvest", + output='screen') + + ld = LaunchDescription() + ld.add_action(declare_points_cmd) + ld.add_action(upf4ros2_navigation_action_cmd) + ld.add_action(upf4ros2_check_wp_action_cmd) + ld.add_action(upf4ros2_collect_action_cmd) + ld.add_action(upf4ros2_demo_cmd) + + return ld diff --git a/upf4ros2_demo/params/house.yaml b/upf4ros2_demo/params/house.yaml index f31c1de..f2cd30c 100644 --- a/upf4ros2_demo/params/house.yaml +++ b/upf4ros2_demo/params/house.yaml @@ -35,4 +35,28 @@ upf4ros2_navigation_action: - "1.85" - "4.38" - "-0.005" - - "1.0" \ No newline at end of file + - "1.0" + + - "wp1" + - "1.6916" + - "-0.05" + - "0.0" + - "1.0" + + - "wp2" + - "2.749" + - "-0.42" + - "0.0" + - "1.0" + + - "wp3" + - "3.6144" + - "-1.32" + - "0.0" + - "1.0" + + - "base" + - "0.3" + - "-0.10" + - "0.0" + - "1.0" diff --git a/upf4ros2_demo/setup.py b/upf4ros2_demo/setup.py index bf49b08..12f4c2e 100644 --- a/upf4ros2_demo/setup.py +++ b/upf4ros2_demo/setup.py @@ -31,8 +31,10 @@ 'upf4ros2_demo1_bash = upf4ros2_demo.upf4ros2_demo1_bash:main', 'upf4ros2_demo2 = upf4ros2_demo.upf4ros2_demo2:main', 'upf4ros2_demo3 = upf4ros2_demo.upf4ros2_demo3:main', + 'upf4ros2_demo3_harvest = upf4ros2_demo.upf4ros2_demo3_harvest:main', 'upf4ros2_navigation_action = upf4ros2_demo.upf4ros2_navigation_action:main', - 'upf4ros2_check_wp_action = upf4ros2_demo.upf4ros2_check_wp_action:main' + 'upf4ros2_check_wp_action = upf4ros2_demo.upf4ros2_check_wp_action:main', + 'upf4ros2_collect_action = upf4ros2_demo.upf4ros2_collect_action:main' ], }, ) diff --git a/upf4ros2_demo/upf4ros2_demo/upf4ros2_check_wp_action.py b/upf4ros2_demo/upf4ros2_demo/upf4ros2_check_wp_action.py index 7db2002..dbd247e 100644 --- a/upf4ros2_demo/upf4ros2_demo/upf4ros2_check_wp_action.py +++ b/upf4ros2_demo/upf4ros2_demo/upf4ros2_check_wp_action.py @@ -41,7 +41,7 @@ def __init__(self): self._get_problem = self.create_client( GetProblem, 'upf4ros2/get_problem') self._set_initial_value = self.create_client( - SetInitialValue, 'upf4ros2/set_initial_value') + SetInitialValue, 'upf4ros2/srv/set_initial_value') self.create_service( CallAction, 'check_wp', self.__execute_callback) diff --git a/upf4ros2_demo/upf4ros2_demo/upf4ros2_collect_action.py b/upf4ros2_demo/upf4ros2_demo/upf4ros2_collect_action.py new file mode 100644 index 0000000..1bba486 --- /dev/null +++ b/upf4ros2_demo/upf4ros2_demo/upf4ros2_collect_action.py @@ -0,0 +1,136 @@ +from typing import List +import rclpy + +from geometry_msgs.msg import Pose +from nav2_msgs.action import NavigateToPose +from upf4ros2_demo_msgs.srv import CallAction +from upf4ros2.ros2_interface_reader import ROS2InterfaceReader +from upf4ros2.ros2_interface_writer import ROS2InterfaceWriter +from unified_planning import model +from unified_planning import shortcuts +from upf_msgs import msg as msgs +from std_msgs.msg import String +from upf4ros2_demo_msgs.srv import CallAction + +from upf_msgs.srv import ( + SetInitialValue, + GetProblem +) + +from simple_node import Node + + +class CollectAction(Node): + + def __init__(self): + super().__init__('upf4ros2_collect_action') + + self._problem_name = 'test' + + self._userType_location = shortcuts.UserType('location') + self._userType_crop = shortcuts.UserType('crop') + self._fluent_collected = model.Fluent( + "collected", + shortcuts.BoolType(), + [model.Parameter('c', self._userType_crop)]) + + self._fluent_is_mature = model.Fluent( + "is_mature", shortcuts.BoolType(), [model.Parameter('c', self._userType_crop)]) + + self._ros2_interface_writer = ROS2InterfaceWriter() + self._ros2_interface_reader = ROS2InterfaceReader() + + self._get_problem = self.create_client( + GetProblem, 'upf4ros2/srv/get_problem') + self._set_initial_value = self.create_client( + SetInitialValue, 'upf4ros2/srv/set_initial_value') + + self.create_service( + CallAction, 'collect', self.__execute_callback) + + def get_problem(self): + """ get actual state of the problem + Args: + + """ + srv = GetProblem.Request() + srv.problem_name = self._problem_name + + self._get_problem.wait_for_service() + self.res = self._get_problem.call(srv) + # problem = self._ros2_interface_reader.convert(self.future.result().problem) + problem = self.res.problem + return problem + + def set_initial_value(self, fluent, object, value_fluent): + """ set initial value to the fluent + Args: + fluent (up.model.Fluent): fluent + object (up.model.Object): fluent's object + value_fluent (bool): new value + """ + srv = SetInitialValue.Request() + srv.problem_name = self._problem_name + srv.expression = self._ros2_interface_writer.convert(fluent(object)) + + item = msgs.ExpressionItem() + item.atom.append(msgs.Atom()) + item.atom[0].boolean_atom.append(value_fluent) + item.type = 'up:bool' + item.kind = msgs.ExpressionItem.CONSTANT + value = msgs.Expression() + value.expressions.append(item) + value.level.append(0) + + srv.value = value + + self._set_initial_value.wait_for_service() + self.future = self._set_initial_value.call(srv) + + self.get_logger().info( + f'Set {fluent.name}({object.name}) with value: {value_fluent}') + + def __execute_callback(self, request, response): + """ srv callback to call the NavigateToPose action + Args: + request (CallAction.Request): request with the action's name and parameters + response (CallAction.Response): response with the result of the action + Returns: + CallAction.Response: response with the result of the action + """ + #self.get_logger().info("entra en collect") + # problem = self._ros2_interface_reader.convert(self.get_problem()) + #self.get_logger().info(f'{request}') + + crop = request.parameters[0].symbol_atom[0] + c = model.Object(crop, self._userType_crop) + + # self.get_logger().info(f'initial values: {problem.initial_values}') + # self.get_logger().info(f'is_mature: {problem.initial_values[self._fluent_is_mature(c)]}') + + # if str(problem.initial_values[self._fluent_is_mature(c)]) == 'true': + + self.get_logger().info("Starting action " + request.action_name) + self.get_logger().info(str(crop) + " collected.") + + self.set_initial_value(self._fluent_collected, c, True) + response.result = True + # else: + # self.get_logger().info("Cannot collect "+str(crop)+" because the is not madure.") + # response.result = False + + return response + + +def main(args=None): + rclpy.init(args=args) + + check_wp_action = CollectAction() + + check_wp_action.join_spin() + + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/upf4ros2_demo/upf4ros2_demo/upf4ros2_demo3_harvest.py b/upf4ros2_demo/upf4ros2_demo/upf4ros2_demo3_harvest.py new file mode 100644 index 0000000..6be2283 --- /dev/null +++ b/upf4ros2_demo/upf4ros2_demo/upf4ros2_demo3_harvest.py @@ -0,0 +1,328 @@ +import rclpy +from rclpy.action import ActionClient +# from rclpy.node import Node +from simple_node import Node + +from unified_planning import model + +from upf4ros2.ros2_interface_reader import ROS2InterfaceReader +from upf4ros2.ros2_interface_writer import ROS2InterfaceWriter +from unified_planning import model +from unified_planning import shortcuts +from unified_planning.shortcuts import Not +from upf_msgs import msg as msgs +from upf4ros2_demo_msgs.srv import CallAction + +from upf_msgs.srv import ( + AddAction, + AddFluent, + AddGoal, + AddObject, + GetProblem, + NewProblem, + SetInitialValue +) + +from upf_msgs.srv import PlanOneShot as PlanOneShotSrv + + +class UPF4ROS2DemoNode(Node): + + def __init__(self): + super().__init__('upf4ros2_demo3_harvest') + + self._problem_name = '' + self._plan_result = {} + + self._ros2_interface_writer = ROS2InterfaceWriter() + self._ros2_interface_reader = ROS2InterfaceReader() + + self._get_problem = self.create_client( + GetProblem, 'upf4ros2/srv/get_problem') + self._new_problem = self.create_client( + NewProblem, 'upf4ros2/srv/new_problem') + self._add_fluent = self.create_client( + AddFluent, 'upf4ros2/srv/add_fluent') + self._add_action = self.create_client( + AddAction, 'upf4ros2/srv/add_action') + self._add_object = self.create_client( + AddObject, 'upf4ros2/srv/add_object') + self._set_initial_value = self.create_client( + SetInitialValue, 'upf4ros2/srv/set_initial_value') + self._add_goal = self.create_client( + AddGoal, 'upf4ros2/add_goal') + self._plan_one_shot_client_srv = self.create_client( + PlanOneShotSrv, 'upf4ros2/srv/planOneShot') + + + def new_problem(self, problem_name): + srv = NewProblem.Request() + srv.problem_name = problem_name + + self._new_problem.wait_for_service() + self.future = self._new_problem.call(srv) + + self._problem_name = problem_name + self.get_logger().info( + f'Create the problem with name: {srv.problem_name}') + + def get_problem(self): + srv = GetProblem.Request() + srv.problem_name = self._problem_name + + self._get_problem.wait_for_service() + self.res = self._get_problem.call(srv) + # problem = self._ros2_interface_reader.convert(self.future.result().problem) + problem = self.res.problem + return problem + + def add_fluent(self, problem, fluent_name, user_types): + list_parameters = [model.Parameter(f"{t.name}_{i}", t) for i, t in enumerate(user_types)] + + fluent = model.Fluent(fluent_name, shortcuts.BoolType(), list_parameters) + srv = AddFluent.Request() + srv.problem_name = self._problem_name + srv.fluent = self._ros2_interface_writer.convert(fluent, problem) + + item = msgs.ExpressionItem() + item.atom.append(msgs.Atom()) + item.atom[0].boolean_atom.append(False) + item.type = 'up:bool' + item.kind = msgs.ExpressionItem.CONSTANT + value = msgs.Expression() + value.expressions.append(item) + value.level.append(0) + + srv.default_value = value + + self._add_fluent.wait_for_service() + self.future = self._add_fluent.call(srv) + + self.get_logger().info(f'Add fluent: {fluent_name}') + return fluent + + def add_object(self, object_name, user_type): + upf_object = model.Object(object_name, user_type) + srv = AddObject.Request() + srv.problem_name = self._problem_name + srv.object = self._ros2_interface_writer.convert(upf_object) + + self._add_object.wait_for_service() + self.future = self._add_object.call(srv) + + self.get_logger().info(f'Add Object: {object_name}') + + return upf_object + + def set_initial_value(self, fluent, objects, value_fluent): + srv = SetInitialValue.Request() + srv.problem_name = self._problem_name + srv.expression = self._ros2_interface_writer.convert(fluent(*objects)) + + item = msgs.ExpressionItem() + item.atom.append(msgs.Atom()) + item.atom[0].boolean_atom.append(value_fluent) + item.type = 'up:bool' + item.kind = msgs.ExpressionItem.CONSTANT + value = msgs.Expression() + value.expressions.append(item) + value.level.append(0) + + srv.value = value + + self._set_initial_value.wait_for_service() + self.future = self._set_initial_value.call(srv) + + self.get_logger().info( + f'Set {fluent.name}({(object.name for object in objects)}) with value: {value_fluent}') + + def add_action(self, action): + srv = AddAction.Request() + srv.problem_name = self._problem_name + srv.action = self._ros2_interface_writer.convert(action) + + self._add_action.wait_for_service() + self.future = self._add_action.call(srv) + + self.get_logger().info(f'Add action: {action.name}') + + def add_goal(self, goal): + srv = AddGoal.Request() + srv.problem_name = self._problem_name + upf_goal = msgs.Goal() + upf_goal.goal = self._ros2_interface_writer.convert(goal) + srv.goal.append(upf_goal) + + self._add_goal.wait_for_service() + self.future = self._add_goal.call(srv) + + self.get_logger().info(f'Set new goal!') + + def __cancel_callback(self, action): + # return to old wp + if action.action_name == 'move': + client = self.create_client(CallAction, action.action_name) + while not client.wait_for_service(timeout_sec=1.0): + self.get_logger().info('service not available, waiting again...') + + req = CallAction.Request() + req.action_name = action.action_name + req.parameters = action.parameters[::-1] + self.res = client.call(req) + if self.res.result: + self.get_logger().info('Return to origin wp') + else: + self.get_logger().info('Can not return to the origin wp!') + + def get_plan_srv(self): + + problem = self.get_problem() + + self.get_logger().info('Planning...') + srv = PlanOneShotSrv.Request() + srv.problem = problem + + self._plan_one_shot_client_srv.wait_for_service() + + self.res = self._plan_one_shot_client_srv.call(srv) + + plan_result = self.res.plan_result + + for action in plan_result.plan.actions: + + params = [x.symbol_atom[0] for x in action.parameters] + self.get_logger().info(action.action_name + "(" + ", ".join(params) + ")") + + for action in plan_result.plan.actions: + + client = self.create_client(CallAction, action.action_name) + while not client.wait_for_service(timeout_sec=1.0): + self.get_logger().info('service not available, waiting again...') + req = CallAction.Request() + req.action_name = action.action_name + req.parameters = action.parameters + + self.res = client.call(req) + + if self.res.result: + self.get_logger().info('Action completed') + else: + self.get_logger().info('Action cancelled!') + self.__cancel_callback(action) + + +def main(args=None): + rclpy.init(args=args) + + # test is the name of the problem for the demo + upf4ros2_demo_node = UPF4ROS2DemoNode() + + upf4ros2_demo_node.new_problem('harvest') + problem = upf4ros2_demo_node._ros2_interface_reader.convert( + upf4ros2_demo_node.get_problem()) + + + # usertype is the type of the fluent's object + # usertype can be 'up:bool', 'up:integer', 'up:integer[]', 'up:real', + # 'up:real[]', shortcuts.UserType('name') + location = shortcuts.UserType('location') + crop = shortcuts.UserType('crop') + + robot_at = upf4ros2_demo_node.add_fluent(problem, 'robot_at', [location]) + wp_checked = upf4ros2_demo_node.add_fluent(problem, 'wp_checked', [location]) + crop_at = upf4ros2_demo_node.add_fluent(problem, 'crop_at', [crop, location]) + is_connected = upf4ros2_demo_node.add_fluent(problem, 'is_connected', [location, location]) + is_mature = upf4ros2_demo_node.add_fluent(problem, 'is_mature', [crop]) + collected = upf4ros2_demo_node.add_fluent(problem, 'collected', [crop]) + + base = upf4ros2_demo_node.add_object('base', location) + wp1 = upf4ros2_demo_node.add_object('wp1', location) + wp2 = upf4ros2_demo_node.add_object('wp2', location) + wp3 = upf4ros2_demo_node.add_object('wp3', location) + tomato1 = upf4ros2_demo_node.add_object('tomato1', crop) + tomato2 = upf4ros2_demo_node.add_object('tomato2', crop) + tomato3 = upf4ros2_demo_node.add_object('tomato3', crop) + + + upf4ros2_demo_node.set_initial_value(robot_at, [base], True) + upf4ros2_demo_node.set_initial_value(is_connected, [base, wp1], True) + upf4ros2_demo_node.set_initial_value(is_connected, [wp1, wp2], True) + upf4ros2_demo_node.set_initial_value(is_connected, [wp2, wp3], True) + upf4ros2_demo_node.set_initial_value(is_connected, [wp3, base], True) + upf4ros2_demo_node.set_initial_value(is_connected, [wp2, base], True) + upf4ros2_demo_node.set_initial_value(crop_at, [tomato1, wp1], True) + upf4ros2_demo_node.set_initial_value(crop_at, [tomato2, wp2], True) + upf4ros2_demo_node.set_initial_value(crop_at, [tomato3, wp3], True) + upf4ros2_demo_node.set_initial_value(is_mature, [tomato3], False) + upf4ros2_demo_node.set_initial_value(is_mature, [tomato2], True) + + # Define navigation action (InstantaneousAction or DurativeAction) + move = model.InstantaneousAction('move', l_from=location, l_to=location) + # move = model.DurativeAction('move', l_from=location, l_to=location) + # If DurativeAction + # Set the action's duration (set_closed_duration_interval, set_open_duration_interval, set_fixed_duration, set_left_open_duration_interval or set_right_open_duration_interval) + # move.set_closed_duration_interval(0, 10) + + l_from = move.parameter('l_from') + l_to = move.parameter('l_to') + + move.add_precondition(robot_at(l_from)) + move.add_precondition(is_connected(l_from, l_to)) + # move.add_condition(model.StartTiming(), robot_at(l_from)) + move.add_effect(robot_at(l_from), False) + move.add_effect(robot_at(l_to), True) + + # ------------------------------------- # + # Define check wp action + check_wp = model.InstantaneousAction('check_wp', wp=location) + wp = check_wp.parameter('wp') + check_wp.add_precondition(robot_at(wp)) + check_wp.add_effect(wp_checked(wp), True) + + # ------------------------------------- # + # Collect action + collect = model.InstantaneousAction('collect', c=crop, l_at=location) + c = collect.parameter('c') + l_at = collect.parameter('l_at') + collect.add_precondition(robot_at(l_at)) + collect.add_precondition(crop_at(c,l_at)) + collect.add_precondition(is_mature(c)) + collect.add_precondition(Not(collected(c))) + collect.add_effect(collected(c), True) + + # ------------------------------------- # + # Unload action + # unload = model.InstantaneousAction('unload', c=crop, l_to=location) + # c_c = unload.parameter('c') + # l = unload.parameter('l_to') + # unload.add_precondition(robot_at(l)) + # unload.add_precondition(collected(c_c)) + # unload.add_effect(collected(c_c), False) + + upf4ros2_demo_node.add_action(move) + upf4ros2_demo_node.add_action(check_wp) + upf4ros2_demo_node.add_action(collect) + + + upf4ros2_demo_node.add_goal(robot_at(base)) + upf4ros2_demo_node.add_goal(collected(tomato2)) + + + problem_old_upf = upf4ros2_demo_node.get_problem() + problem_old = upf4ros2_demo_node._ros2_interface_reader.convert( + problem_old_upf) + upf4ros2_demo_node.get_logger().info(f'{problem_old}') + + upf4ros2_demo_node.get_plan_srv() + + problem_updated = upf4ros2_demo_node._ros2_interface_reader.convert( + upf4ros2_demo_node.get_problem()) + upf4ros2_demo_node.get_logger().info(f'{problem_updated}') + + upf4ros2_demo_node.join_spin() + + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/upf4ros2_demo/upf4ros2_demo/upf4ros2_navigation_action.py b/upf4ros2_demo/upf4ros2_demo/upf4ros2_navigation_action.py index 04eccad..f1d2a81 100644 --- a/upf4ros2_demo/upf4ros2_demo/upf4ros2_navigation_action.py +++ b/upf4ros2_demo/upf4ros2_demo/upf4ros2_navigation_action.py @@ -53,7 +53,7 @@ def __init__(self): '/navigate_to_pose') self._set_initial_value = self.create_client( - SetInitialValue, 'upf4ros2/set_initial_value') + SetInitialValue, 'upf4ros2/srv/set_initial_value') self.create_service( CallAction, 'move', self.__execute_callback) @@ -135,6 +135,7 @@ def __execute_callback(self, request, response): self.get_logger().info("Goal to " + str(wp) + " done") self.set_initial_value(self._fluent, l1, False) self.set_initial_value(self._fluent, l2, True) + response.result = True else: self.get_logger().info("Goal to " + str(wp) + " was rejected!")