├── .github └── workflows │ └── main.yaml ├── .gitignore ├── README.md ├── docs ├── Doxyfile └── README.md ├── plansys2_upf_plan_solver ├── CMakeLists.txt ├── README.md ├── include │ └── plansys2_upf_plan_solver │ │ └── upf_plan_solver.hpp ├── package.xml ├── plansys2_upf_plan_solver_plugin.xml ├── src │ └── plansys2_upf_plan_solver │ │ └── upf_plan_solver.cpp └── test │ ├── CMakeLists.txt │ ├── pddl │ ├── domain_1_ok.pddl │ ├── domain_2_error.pddl │ ├── domain_simple.pddl │ ├── problem_simple_1.pddl │ ├── problem_simple_2.pddl │ └── problem_simple_3.pddl │ └── unit │ ├── CMakeLists.txt │ └── upf_test.cpp ├── upf.repos ├── upf4ros2 ├── launch │ └── upf4ros2.launch.py ├── package.xml ├── resource │ └── upf4ros2 ├── setup.cfg ├── setup.py ├── tests │ ├── pddl │ │ ├── blocks_domain.pddl │ │ ├── blocks_problem_0.pddl │ │ ├── blocks_problem_1.pddl │ │ ├── domain_tt.pddl │ │ ├── gripper_domain.pddl │ │ ├── gripper_problem_0.pddl │ │ └── problem_tt_1.pddl │ ├── test_conversion.py │ ├── test_copyright.py │ ├── test_flake8.py │ ├── test_pep257.py │ └── test_upf4ros2.py └── upf4ros2 │ ├── __init__.py │ ├── converter.py │ ├── ros2_interface_reader.py │ ├── ros2_interface_writer.py │ ├── ros2_utils.py │ └── upf4ros2_main.py ├── upf4ros2_demo ├── README.md ├── launch │ ├── upf4ros2_demo1.launch.py │ ├── upf4ros2_demo1_bash.launch.py │ ├── upf4ros2_demo1_pddlfile.launch.py │ ├── upf4ros2_demo2.launch.py │ └── upf4ros2_demo3.launch.py ├── package.xml ├── params │ └── house.yaml ├── resource │ ├── upf4ros2_demo │ └── upf_problem.sh ├── setup.cfg ├── setup.py ├── test │ ├── pddl │ │ ├── domain.pddl │ │ ├── domain_check_door.pddl │ │ ├── problem.pddl │ │ └── problem_check_door.pddl │ ├── test_copyright.py │ ├── test_flake8.py │ └── test_pep257.py └── upf4ros2_demo │ ├── __init__.py │ ├── upf4ros2_check_wp_action.py │ ├── upf4ros2_demo1.py │ ├── upf4ros2_demo1_bash.py │ ├── upf4ros2_demo1_pddl.py │ ├── upf4ros2_demo2.py │ ├── upf4ros2_demo3.py │ └── upf4ros2_navigation_action.py ├── upf4ros2_demo_msgs ├── CMakeLists.txt ├── package.xml └── srv │ └── CallAction.srv └── upf_msgs ├── CMakeLists.txt ├── action ├── PDDLPlanOneShot.action ├── PlanOneShot.action └── PlanOneShotRemote.action ├── msg ├── AbstractTaskDeclaration.msg ├── Action.msg ├── ActionInstance.msg ├── Assignment.msg ├── Atom.msg ├── CompilerResult.msg ├── Condition.msg ├── Duration.msg ├── Effect.msg ├── EffectExpression.msg ├── Expression.msg ├── ExpressionItem.msg ├── Fluent.msg ├── Goal.msg ├── GoalWithCost.msg ├── Hierarchy.msg ├── Interval.msg ├── LogMessage.msg ├── Method.msg ├── Metric.msg ├── ObjectDeclaration.msg ├── PDDLPlanRequest.msg ├── Parameter.msg ├── Plan.msg ├── PlanGenerationResult.msg ├── PlanRequest.msg ├── PlanRequestRemote.msg ├── Problem.msg ├── Real.msg ├── Schedule.msg ├── Task.msg ├── TaskNetwork.msg ├── TimeInterval.msg ├── TimedEffect.msg ├── Timepoint.msg ├── Timing.msg ├── TypeDeclaration.msg ├── ValidationMetric.msg ├── ValidationRequest.msg ├── ValidationResult.msg └── Variable.msg ├── package.xml └── srv ├── AddAction.srv ├── AddFluent.srv ├── AddGoal.srv ├── AddObject.srv ├── GetProblem.srv ├── NewProblem.srv ├── PDDLPlanOneShot.srv ├── PlanOneShot.srv ├── SetInitialValue.srv └── SetProblem.srv /.github/workflows/main.yaml: -------------------------------------------------------------------------------- 1 | name: main 2 | 3 | on: 4 | pull_request: 5 | branches: 6 | - main 7 | push: 8 | branches: 9 | - main 10 | 11 | env: 12 | up_tamer_commit: "869e7ab06cf23c5541a47f46209159fd51d8021f" 13 | up_pyperplan_commit: "ac2b04d2d41c20b15f7c4143c19947f9704d1888" 14 | up_fast_downward_commit: "eb1f0d801e140fffbdd6c1eb2b13e483299ab240" 15 | 16 | jobs: 17 | build-and-test: 18 | runs-on: ${{ matrix.os }} 19 | container: 20 | image: osrf/ros:humble-desktop 21 | strategy: 22 | matrix: 23 | os: [ubuntu-22.04] 24 | fail-fast: false 25 | steps: 26 | - name: Checkout repository 27 | uses: actions/checkout@v4 28 | - name: Install deps 29 | 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 30 | - name: fix pytest name 31 | run: sudo ln -s /usr/bin/pytest-3 /usr/bin/pytest 32 | - name: Install unified planning from sources 33 | run: | 34 | git clone https://github.com/aiplan4eu/unified-planning.git 35 | cd unified-planning 36 | python3 -m pip install -r requirements.txt 37 | python3 -m pip install -r dev-requirements.txt 38 | python3 -m pip install tarski[arithmetic] 39 | sudo apt install -y gringo 40 | python3 -m pip install black==22.6.0 41 | python3 -m black --check --exclude=unified_planning/grpc/generated/ . 42 | python3 -m mypy unified_planning 43 | python3 scripts/test_imports.py 44 | cd .. 45 | git clone https://github.com/aiplan4eu/up-tamer 46 | python3 -m pip install up-tamer/ 47 | git clone https://github.com/aiplan4eu/up-pyperplan 48 | python3 -m pip install up-pyperplan/ 49 | pip install unified_planning[tamer] 50 | - name: Create custom repos 51 | run: cp upf.repos /tmp/all.repos 52 | - name: build and test 53 | uses: ros-tooling/action-ros-ci@0.3.15 54 | with: 55 | package-name: upf4ros2 upf_msgs 56 | target-ros2-distro: humble 57 | vcs-repo-file-url: /tmp/all.repos 58 | colcon-defaults: | 59 | { 60 | "test": { 61 | "parallel-workers" : 4 62 | } 63 | } 64 | colcon-mixin-name: coverage-gcc 65 | colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/main/index.yaml 66 | extra-args: "--event-handlers console_direct+ --return-code-on-test-failure" 67 | - name: Codecov 68 | uses: codecov/codecov-action@v1.2.1 69 | with: 70 | file: ros_ws/lcov/total_coverage.info 71 | flags: unittests 72 | name: codecov-umbrella 73 | # yml: ./codecov.yml 74 | fail_ci_if_error: false 75 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | *.pyc 6 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # UPF4ROS2 2 | 3 | [![main](https://github.com/igonzf/UPF4ROS2/actions/workflows/main.yaml/badge.svg)](https://github.com/igonzf/UPF4ROS2/actions/workflows/main.yaml) 4 | 5 | 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. 6 | 7 | ## Install and building 8 | 9 | ``` 10 | $ pip install --pre unified-planning[pyperplan,tamer] 11 | $ pip install ConfigSpace 12 | $ pip install typing_extensions==4.7.1 --upgrade 13 | $ cd 14 | $ cd src 15 | $ git clone https://github.com/PlanSys2/UPF4ROS2.git 16 | $ vcs import . < UPF4ROS2/upf.repos 17 | $ cd .. 18 | $ colcon build --symlink-install 19 | ``` 20 | 21 | ### Install UPF from sources 22 | 23 | ``` 24 | $ cd src 25 | $ git clone https://github.com/aiplan4eu/unified-planning.git 26 | $ cd unified-planning 27 | $ python3 -m pip install -r requirements.txt 28 | $ python3 -m pip install -r dev-requirements.txt 29 | $ python3 -m pip install tarski[arithmetic] 30 | $ sudo apt install -y gringo 31 | $ python3 -m pip install black==22.6.0 32 | $ python3 -m black --check --exclude=unified_planning/grpc/generated/ . 33 | $ python3 -m mypy unified_planning 34 | $ python3 scripts/test_imports.py 35 | $ cd .. 36 | $ git clone -b 869e7ab06cf23c5541a47f46209159fd51d8021f https://github.com/aiplan4eu/up-tamer 37 | $ python3 -m pip install up-tamer/ 38 | $ git clone -b ac2b04d2d41c20b15f7c4143c19947f9704d1888 https://github.com/aiplan4eu/up-pyperplan 39 | $ python3 -m pip install up-pyperplan/ 40 | ``` 41 | 42 | Install Java 17, with: 43 | 44 | ``` 45 | $ sudo apt install openjdk-17-jdk openjdk-17-jre 46 | ``` 47 | 48 | ``` 49 | $ git clone https://gitlab.com/enricos83/ENHSP-Public.git 50 | $ cd ENHSP-Public; git checkout enhsp20-0.9.5; ./compile; cd .. 51 | $ mkdir .planners; mv ENHSP-Public/enhsp-dist .planners/enhsp-20; rm -rf ENHSP-Public 52 | ``` 53 | 54 | Check with : 55 | 56 | ``` 57 | $ cd unified-planning 58 | $ bash run_tests.sh 59 | ``` 60 | 61 | ## Usage 62 | 63 | `$ ros2 launch upf4ros2 upf4ros2.launch.py` 64 | 65 | ## Nodes 66 | 67 | - **upf4ros** 68 | - Services: 69 | - `/upf4ros2/add_action` `[upf_msgs/srv/AddAction]` 70 | - `/upf4ros2/add_fluent` `[upf_msgs/srv/AddFluent]` 71 | - `/upf4ros2/add_goal` `[upf_msgs/srv/AddGoal]` 72 | - `/upf4ros2/add_object` `[upf_msgs/srv/AddObject]` 73 | - `/upf4ros2/new_problem` [upf_msgs/srv/NewProblem]` 74 | - `/upf4ros2/set_initial_value` [upf_msgs/srv/SetInitialValue]` 75 | - `/upf4ros2/set_problem` [upf_msgs/srv/SetProblem]` 76 | - Actions: 77 | - `/upf4ros2/planOneShotPDDL` `[upf_msgs/action/PDDLPlanOneShot]` 78 | - `/upf4ros2/planOneShot` `[upf_msgs/action/PlanOneShot]` 79 | - `/upf4ros2/planOneShotRemote` `[upf_msgs/action/PlanOneShotRemote]` 80 | 81 | ## Demo 82 | 83 | ### [Demo 1](https://www.youtube.com/watch?v=fObz6H1DnXs) 84 | 85 | This demo consists of creating the problem from a ros2 node to navigate from living room to the entrance. 86 | 87 | `$ ros2 launch upf4ros2 upf4ros2.launch.py` 88 | 89 | `$ ros2 launch upf4ros2_demo upf4ros2_demo1.launch.py` 90 | 91 | ### Demo 1 (pddl file) 92 | 93 | This demo consists of creating the problem from a pddl domain and problem file. 94 | 95 | `$ ros2 launch upf4ros2 upf4ros2.launch.py` 96 | 97 | `$ ros2 launch upf4ros2_demo upf4ros2_demo1_pddlfile.launch.py` 98 | 99 | ### Demo 1 (bash) 100 | 101 | This demo consists of creating the problem from the command line. For easier use you can use the script in /upf4ros2_demo/resource/upf_problem.sh 102 | 103 | `$ ros2 launch upf4ros2 upf4ros2.launch.py` 104 | 105 | `$ ./upf_problem.sh` 106 | 107 | `$ ros2 launch upf4ros2_demo upf4ros2_demo1_bash.launch.py` 108 | 109 | ### [Demo 2](https://www.youtube.com/watch?v=HJ46htSfPZY) 110 | 111 | This demo consists of creating the problem from a ROS 2 node to navigate from living room to the entrance. 112 | For run this demo I used the simulated TIAGo robot from this [repo](https://github.com/jmguerreroh/ros2_computer_vision) 113 | 114 | `$ ros2 launch upf4ros2 upf4ros2.launch.py` 115 | 116 | `$ ros2 launch upf4ros2_demo upf4ros2_demo2.launch.py` 117 | 118 | ### Demo 3 119 | 120 | This demo consists of creating the problem from a ROS 2 node to navigate and check a list of waypoints starting from living room. 121 | For run this demo I used the simulated TIAGo robot from this [repo](https://github.com/jmguerreroh/ros2_computer_vision) 122 | 123 | `$ ros2 launch upf4ros2 upf4ros2.launch.py` 124 | 125 | `$ ros2 launch upf4ros2_demo upf4ros2_demo3.launch.py` 126 | 127 | There are two alternatives: 128 | 129 | - Regular case: Illustrated in this [video](https://youtu.be/2nKqxGYlHk8) 130 | 131 | - Replanning case: one of the waypoints is not reachable and it is necessary to replan. Illustrated in this [video](https://youtu.be/UJncg7GPCro) 132 | 133 | ## Acknowledgments 134 | 135 | 136 | 137 | This library is being developed for the AIPlan4EU H2020 project (https://aiplan4eu-project.eu) that is funded by the European Commission under grant agreement number 101016442. 138 | -------------------------------------------------------------------------------- /docs/README.md: -------------------------------------------------------------------------------- 1 | ## UPF4ROS2 2 | 3 | ## Instalation 4 | 5 | ### Dependencies: 6 | 7 | - Unified Planning 8 | 9 | $ pip install --pre unified-planning[pyperplan,tamer] 10 | 11 | - Unified Planning sources 12 | 13 | $ vcs import . < upf.repos 14 | 15 | ### Extra (WIP) 16 | 17 | - Documentation with Doxygen: 18 | 19 | Install Doxygen package and run command in docs folder 20 | 21 | 22 | $ doxygen 23 | 24 | 25 | ### Tests 26 | 27 | Standalone option (Double check your ROS 2 working directory ) 28 | 29 | $ python3 ./src/UPF4ROS2/upf4ros2/test/test_conversion.py 30 | $ python3 ./src/UPF4ROS2/upf4ros2/test/test_upf4ros2.py 31 | 32 | ### Launch 33 | 34 | $ ros2 launch upf4ros2 upf4ros2.launch.py 35 | 36 | -------------------------------------------------------------------------------- /plansys2_upf_plan_solver/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(plansys2_upf_plan_solver) 2 | 3 | cmake_minimum_required(VERSION 3.5) 4 | 5 | find_package(ament_cmake REQUIRED) 6 | find_package(rclcpp REQUIRED) 7 | find_package(rclcpp_action REQUIRED) 8 | find_package(ament_index_cpp REQUIRED) 9 | find_package(plansys2_core REQUIRED) 10 | find_package(pluginlib REQUIRED) 11 | find_package(upf_msgs REQUIRED) 12 | 13 | set(CMAKE_CXX_STANDARD 17) 14 | 15 | set(dependencies 16 | rclcpp 17 | rclcpp_action 18 | ament_index_cpp 19 | plansys2_core 20 | pluginlib 21 | upf_msgs 22 | ) 23 | 24 | include_directories( 25 | include 26 | ) 27 | 28 | add_library(${PROJECT_NAME} SHARED 29 | src/plansys2_upf_plan_solver/upf_plan_solver.cpp 30 | ) 31 | ament_target_dependencies(${PROJECT_NAME} ${dependencies}) 32 | 33 | target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") 34 | 35 | pluginlib_export_plugin_description_file(plansys2_core plansys2_upf_plan_solver_plugin.xml) 36 | 37 | install(DIRECTORY include/ 38 | DESTINATION include/ 39 | ) 40 | 41 | install(FILES plansys2_upf_plan_solver_plugin.xml 42 | DESTINATION share/${PROJECT_NAME} 43 | ) 44 | 45 | install(TARGETS 46 | ${PROJECT_NAME} 47 | ARCHIVE DESTINATION lib 48 | LIBRARY DESTINATION lib 49 | RUNTIME DESTINATION lib/${PROJECT_NAME} 50 | ) 51 | 52 | if(BUILD_TESTING) 53 | find_package(ament_lint_auto REQUIRED) 54 | ament_lint_auto_find_test_dependencies() 55 | 56 | find_package(ament_cmake_gtest REQUIRED) 57 | find_package(launch_testing_ament_cmake) 58 | add_subdirectory(test) 59 | endif() 60 | 61 | ament_export_include_directories(include) 62 | ament_export_libraries(${PROJECT_NAME}) 63 | ament_export_dependencies(${dependencies}) 64 | 65 | ament_package() -------------------------------------------------------------------------------- /plansys2_upf_plan_solver/README.md: -------------------------------------------------------------------------------- 1 | # UPF Plan solver 2 | 3 | This package contains a plan solver that uses [Unified Planning Framework](https://github.com/aiplan4eu/unified-planning) for solving PDDL plans. 4 | -------------------------------------------------------------------------------- /plansys2_upf_plan_solver/include/plansys2_upf_plan_solver/upf_plan_solver.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Intelligent Robotics Lab 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef PLANSYS2_UPF_PLAN_SOLVER__UPF_PLAN_SOLVER_HPP_ 16 | #define PLANSYS2_UPF_PLAN_SOLVER__UPF_PLAN_SOLVER_HPP_ 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | #include "plansys2_core/PlanSolverBase.hpp" 23 | #include "rclcpp_action/rclcpp_action.hpp" 24 | #include "upf_msgs/action/pddl_plan_one_shot.hpp" 25 | 26 | namespace plansys2 27 | { 28 | 29 | class UPFPlanSolver : public PlanSolverBase 30 | { 31 | private: 32 | using PDDLPlanOneShot = upf_msgs::action::PDDLPlanOneShot; 33 | 34 | std::string parameter_name_; 35 | rclcpp_lifecycle::LifecycleNode::SharedPtr lc_node_; 36 | rclcpp::Node::SharedPtr service_node_; 37 | 38 | plansys2_msgs::msg::Plan convert(const upf_msgs::msg::Plan & plan); 39 | plansys2_msgs::msg::PlanItem convert(const upf_msgs::msg::ActionInstance & action); 40 | 41 | public: 42 | UPFPlanSolver(); 43 | 44 | void configure(rclcpp_lifecycle::LifecycleNode::SharedPtr &, const std::string &); 45 | 46 | std::optional getPlan( 47 | const std::string & domain, const std::string & problem, 48 | const std::string & node_namespace = ""); 49 | 50 | bool is_valid_domain( 51 | const std::string & domain, 52 | const std::string & node_namespace = ""); 53 | }; 54 | 55 | } // namespace plansys2 56 | 57 | #endif // PLANSYS2_UPF_PLAN_SOLVER__UPF_PLAN_SOLVER_HPP_ 58 | -------------------------------------------------------------------------------- /plansys2_upf_plan_solver/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | plansys2_upf_plan_solver 5 | 2.0.8 6 | 7 | This package contains the PDDL-based Planner module for the ROS2 Planning System using UPF 8 | 9 | Francisco Martin Rico 10 | 11 | Apache License, Version 2.0 12 | 13 | ament_cmake 14 | 15 | rclcpp 16 | rclcpp_action 17 | ament_index_cpp 18 | plansys2_core 19 | pluginlib 20 | upf_msgs 21 | 22 | upf4ros2 23 | 24 | ament_lint_common 25 | ament_lint_auto 26 | ament_cmake_gtest 27 | ros2run 28 | 29 | 30 | ament_cmake 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /plansys2_upf_plan_solver/plansys2_upf_plan_solver_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /plansys2_upf_plan_solver/src/plansys2_upf_plan_solver/upf_plan_solver.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2019 Intelligent Robotics Lab 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #include "upf_msgs/srv/pddl_plan_one_shot.hpp" 26 | #include "upf_msgs/msg/pddl_plan_request.hpp" 27 | #include "upf_msgs/msg/plan_generation_result.hpp" 28 | 29 | #include "plansys2_upf_plan_solver/upf_plan_solver.hpp" 30 | 31 | namespace plansys2 32 | { 33 | 34 | using namespace std::chrono_literals; 35 | 36 | UPFPlanSolver::UPFPlanSolver() 37 | { 38 | } 39 | 40 | plansys2_msgs::msg::PlanItem 41 | UPFPlanSolver::convert(const upf_msgs::msg::ActionInstance & action) 42 | { 43 | plansys2_msgs::msg::PlanItem ret; 44 | 45 | float start_time = action.start_time.numerator / action.start_time.denominator; 46 | float end_time = action.end_time.numerator / action.end_time.denominator; 47 | 48 | ret.time = action.start_time.numerator / action.start_time.denominator; 49 | ret.duration = end_time - start_time; 50 | 51 | ret.action = "(" + action.action_name; 52 | for (const auto & parameter : action.parameters) { 53 | if (!parameter.symbol_atom.empty()) { 54 | ret.action = ret.action + " " + parameter.symbol_atom[0]; 55 | } else if (!parameter.int_atom.empty()) { 56 | ret.action = ret.action + " " + std::to_string(parameter.int_atom[0]); 57 | } else if (!parameter.real_atom.empty()) { 58 | float float_value = parameter.real_atom[0].numerator / parameter.real_atom[0].denominator; 59 | ret.action = ret.action + " " + std::to_string(float_value); 60 | } else if (!parameter.boolean_atom.empty()) { 61 | ret.action = ret.action + " " + (parameter.boolean_atom[0]? "true" : "false"); 62 | } 63 | } 64 | ret.action = ret.action + ")"; 65 | 66 | return ret; 67 | } 68 | 69 | plansys2_msgs::msg::Plan 70 | UPFPlanSolver::convert(const upf_msgs::msg::Plan & plan) 71 | { 72 | plansys2_msgs::msg::Plan ret; 73 | 74 | for (const auto & action : plan.actions) { 75 | ret.items.push_back(convert(action)); 76 | } 77 | 78 | return ret; 79 | } 80 | 81 | void UPFPlanSolver::configure( 82 | rclcpp_lifecycle::LifecycleNode::SharedPtr & lc_node, 83 | const std::string & plugin_name) 84 | { 85 | parameter_name_ = plugin_name + ".arguments"; 86 | lc_node_ = lc_node; 87 | lc_node_->declare_parameter(parameter_name_, ""); 88 | service_node_ = rclcpp::Node::make_shared(plugin_name + "_node"); 89 | } 90 | 91 | std::optional 92 | UPFPlanSolver::getPlan( 93 | const std::string & domain, const std::string & problem, 94 | const std::string & node_namespace) 95 | { 96 | if (system(nullptr) == 0) { 97 | return {}; 98 | } 99 | 100 | if (node_namespace != "") { 101 | std::filesystem::path tp = std::filesystem::temp_directory_path(); 102 | for (auto p : std::filesystem::path(node_namespace) ) { 103 | if (p != std::filesystem::current_path().root_directory()) { 104 | tp /= p; 105 | } 106 | } 107 | std::filesystem::create_directories(tp); 108 | } 109 | 110 | std::ofstream domain_out("/tmp/" + node_namespace + "/domain.pddl"); 111 | domain_out << domain; 112 | domain_out.close(); 113 | 114 | std::ofstream problem_out("/tmp/" + node_namespace + "/problem.pddl"); 115 | problem_out << problem; 116 | problem_out.close(); 117 | 118 | auto client = service_node_->create_client( 119 | "upf4ros2/srv/planOneShotPDDL"); 120 | 121 | 122 | while (!client->wait_for_service(1s)) { 123 | if (!rclcpp::ok()) { 124 | RCLCPP_ERROR( 125 | service_node_->get_logger(), 126 | "Interrupted while waiting for the service. Exiting."); 127 | return {}; 128 | } 129 | RCLCPP_WARN(service_node_->get_logger(), "service not available, waiting again..."); 130 | } 131 | 132 | auto request = std::make_shared(); 133 | request->plan_request.mode = upf_msgs::msg::PDDLPlanRequest::FILE; 134 | request->plan_request.domain = "/tmp/" + node_namespace + "/domain.pddl"; 135 | request->plan_request.problem = "/tmp/" + node_namespace + "/problem.pddl"; 136 | 137 | auto result = client->async_send_request(request); 138 | 139 | if (rclcpp::spin_until_future_complete(service_node_, result) != 140 | rclcpp::FutureReturnCode::SUCCESS) 141 | { 142 | RCLCPP_ERROR( 143 | service_node_->get_logger(), 144 | "Failed to call service upf4ros2/srv/planOneShotPDDL"); 145 | return {}; 146 | } 147 | 148 | auto res = *result.get(); 149 | upf_msgs::msg::PlanGenerationResult & plan_result = res.plan_result; 150 | 151 | if (!res.success || 152 | (plan_result.status != upf_msgs::msg::PlanGenerationResult::SOLVED_SATISFICING && 153 | plan_result.status != upf_msgs::msg::PlanGenerationResult::SOLVED_OPTIMALLY)) 154 | { 155 | RCLCPP_ERROR( 156 | service_node_->get_logger(), 157 | "Failed resolving plan %s", res.message.c_str()); 158 | return {}; 159 | } 160 | 161 | plansys2_msgs::msg::Plan ret = convert(plan_result.plan); 162 | 163 | return ret; 164 | } 165 | 166 | bool 167 | UPFPlanSolver::is_valid_domain( 168 | const std::string & domain, 169 | const std::string & node_namespace) 170 | { 171 | if (system(nullptr) == 0) { 172 | return false; 173 | } 174 | 175 | std::filesystem::path temp_dir = std::filesystem::temp_directory_path(); 176 | if (node_namespace != "") { 177 | for (auto p : std::filesystem::path(node_namespace) ) { 178 | if (p != std::filesystem::current_path().root_directory()) { 179 | temp_dir /= p; 180 | } 181 | } 182 | std::filesystem::create_directories(temp_dir); 183 | } 184 | 185 | std::ofstream domain_out(temp_dir.string() + "/check_domain.pddl"); 186 | domain_out << domain; 187 | domain_out.close(); 188 | 189 | std::ofstream problem_out(temp_dir.string() + "/check_problem.pddl"); 190 | problem_out << "(define (problem void) (:domain plansys2))"; 191 | problem_out.close(); 192 | 193 | int status = system( 194 | ("ros2 run popf popf " + temp_dir.string() + "/check_domain.pddl " + temp_dir.string() + 195 | "/check_problem.pddl > " + temp_dir.string() + "/check.out").c_str()); 196 | 197 | if (status == -1) { 198 | return false; 199 | } 200 | 201 | std::string line; 202 | std::ifstream plan_file(temp_dir.string() + "/check.out"); 203 | bool solution = false; 204 | 205 | if (plan_file && plan_file.is_open()) { 206 | while (getline(plan_file, line)) { 207 | if (!solution) { 208 | if (line.find("Solution Found") != std::string::npos) { 209 | solution = true; 210 | } 211 | } 212 | } 213 | plan_file.close(); 214 | } 215 | 216 | return solution; 217 | } 218 | 219 | } // namespace plansys2 220 | 221 | #include "pluginlib/class_list_macros.hpp" 222 | PLUGINLIB_EXPORT_CLASS(plansys2::UPFPlanSolver, plansys2::PlanSolverBase); 223 | -------------------------------------------------------------------------------- /plansys2_upf_plan_solver/test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(TEST_PDDL_DIR ${CMAKE_CURRENT_SOURCE_DIR}/pddl) 2 | set(TEST_LAUNCH_DIR ${CMAKE_CURRENT_SOURCE_DIR}/test_launch_files) 3 | 4 | install(DIRECTORY 5 | ${TEST_PDDL_DIR} 6 | DESTINATION share/${PROJECT_NAME} 7 | ) 8 | 9 | add_subdirectory(unit) 10 | #add_subdirectory(integration) -------------------------------------------------------------------------------- /plansys2_upf_plan_solver/test/pddl/domain_1_ok.pddl: -------------------------------------------------------------------------------- 1 | (define (domain plansys2) 2 | (:requirements :strips :typing :adl :fluents :durative-actions :negative-preconditions) 3 | 4 | ;; Types ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; 5 | (:types 6 | person 7 | message 8 | robot 9 | room 10 | );; end Types ;;;;;;;;;;;;;;;;;;;;;;;;; 11 | 12 | ;; Predicates ;;;;;;;;;;;;;;;;;;;;;;;;; 13 | (:predicates 14 | 15 | (robot_talk ?r - robot ?m - message ?p - person) 16 | (robot_near_person ?r - robot ?p - person) 17 | (robot_at ?r - robot ?ro - room) 18 | (person_at ?p - person ?ro - room) 19 | 20 | );; end Predicates ;;;;;;;;;;;;;;;;;;;; 21 | ;; Functions ;;;;;;;;;;;;;;;;;;;;;;;;; 22 | (:functions 23 | 24 | );; end Functions ;;;;;;;;;;;;;;;;;;;; 25 | ;; Actions ;;;;;;;;;;;;;;;;;;;;;;;;;;;; 26 | (:durative-action move 27 | :parameters (?r - robot ?r1 ?r2 - room) 28 | :duration ( = ?duration 5) 29 | :condition (and 30 | (at start(robot_at ?r ?r1)) 31 | (at start(not(robot_at ?r ?r2)))) 32 | :effect (and 33 | (at start(not(robot_at ?r ?r1))) 34 | (at end(robot_at ?r ?r2)) 35 | ) 36 | ) 37 | 38 | (:durative-action talk 39 | :parameters (?r - robot ?p - person ?m - message) 40 | :duration ( = ?duration 5) 41 | :condition (and 42 | (over all(robot_near_person ?r ?p)) 43 | ) 44 | :effect (and 45 | (at end(robot_talk ?r ?m ?p)) 46 | ) 47 | ) 48 | 49 | (:durative-action approach 50 | :parameters (?r - robot ?ro - room ?p - person) 51 | :duration ( = ?duration 5) 52 | :condition (and 53 | (over all(robot_at ?r ?ro)) 54 | (over all(person_at ?p ?ro)) 55 | ) 56 | :effect (and 57 | (at end(robot_near_person ?r ?p)) 58 | ) 59 | ) 60 | 61 | 62 | 63 | );; end Domain ;;;;;;;;;;;;;;;;;;;;;;;; 64 | -------------------------------------------------------------------------------- /plansys2_upf_plan_solver/test/pddl/domain_2_error.pddl: -------------------------------------------------------------------------------- 1 | (define (domain plansys2) 2 | (:requirements :strips :typing :adl :fluents :durative-actions :negative-preconditions) 3 | 4 | ;; Types ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; 5 | (:types 6 | person 7 | message 8 | robot 9 | room 10 | );; end Types ;;;;;;;;;;;;;;;;;;;;;;;;; 11 | 12 | ;; Predicates ;;;;;;;;;;;;;;;;;;;;;;;;; 13 | (:predicates 14 | 15 | (robot_talk ?r - robot ?m - message ?p - person) 16 | (robot_near_person ?r - robot ?p - person) 17 | (robot_at ?r - robot ?ro - room) 18 | (person_at ?p - person ?ro - room) 19 | 20 | );; end Predicates ;;;;;;;;;;;;;;;;;;;; 21 | ;; Functions ;;;;;;;;;;;;;;;;;;;;;;;;; 22 | (:functions 23 | 24 | );; end Functions ;;;;;;;;;;;;;;;;;;;; 25 | ;; Actions ;;;;;;;;;;;;;;;;;;;;;;;;;;;; 26 | (:durative-action move 27 | :parameters (?r - robot ?r1 ?r2 - room) 28 | :duration ( = ?duration 5) 29 | :condition (and 30 | (at start(robot_at ?r ?r1)) 31 | (at start(not(robot_at ?r ?r2)))) 32 | :effect (and 33 | (at start(not(robot_at ?r ?r1))) 34 | (at end(robot_at ?r ?r2)) 35 | ) 36 | ) 37 | 38 | (:durative-action talk 39 | :parameters (?r - robot ?p - person ?m - message) 40 | :duration ( = ?duration 5) 41 | :condition (and 42 | (over all(robot_near_person ?r ?p)) 43 | ) 44 | :effect (and 45 | (at end(robot_talk ?r ?m ?p)) 46 | ) 47 | 48 | 49 | (:durative-action approach 50 | :parameters (?r - robot ?ro - room ?p - person) 51 | :duration ( = ?duration 5) 52 | :condition (and 53 | (over all(robot_at ?r ?ro)) 54 | (over all(person_at ?p ?ro)) 55 | ) 56 | :effect (and 57 | (at end(robot_near_person ?r ?p)) 58 | ) 59 | ) 60 | 61 | 62 | 63 | );; end Domain ;;;;;;;;;;;;;;;;;;;;;;;; 64 | -------------------------------------------------------------------------------- /plansys2_upf_plan_solver/test/pddl/domain_simple.pddl: -------------------------------------------------------------------------------- 1 | (define (domain simple) 2 | (:requirements :strips :typing :adl :fluents :durative-actions) 3 | 4 | ;; Types ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; 5 | (:types 6 | person 7 | message 8 | robot 9 | room 10 | );; end Types ;;;;;;;;;;;;;;;;;;;;;;;;; 11 | 12 | ;; Predicates ;;;;;;;;;;;;;;;;;;;;;;;;; 13 | (:predicates 14 | 15 | (robot_talk ?r - robot ?m - message ?p - person) 16 | (robot_near_person ?r - robot ?p - person) 17 | (robot_at ?r - robot ?ro - room) 18 | (person_at ?p - person ?ro - room) 19 | 20 | );; end Predicates ;;;;;;;;;;;;;;;;;;;; 21 | ;; Functions ;;;;;;;;;;;;;;;;;;;;;;;;; 22 | 23 | ;; Actions ;;;;;;;;;;;;;;;;;;;;;;;;;;;; 24 | (:durative-action move 25 | :parameters (?r - robot ?r1 ?r2 - room) 26 | :duration ( = ?duration 5) 27 | :condition (and 28 | (at start(robot_at ?r ?r1))) 29 | :effect (and 30 | (at start(not(robot_at ?r ?r1))) 31 | (at end(robot_at ?r ?r2)) 32 | ) 33 | ) 34 | 35 | (:durative-action talk 36 | :parameters (?r - robot ?from ?p - person ?m - message) 37 | :duration ( = ?duration 5) 38 | :condition (and 39 | (over all(robot_near_person ?r ?p)) 40 | ) 41 | :effect (and 42 | (at end(robot_talk ?r ?m ?p)) 43 | ) 44 | ) 45 | 46 | (:durative-action approach 47 | :parameters (?r - robot ?ro - room ?p - person) 48 | :duration ( = ?duration 5) 49 | :condition (and 50 | (over all(robot_at ?r ?ro)) 51 | (over all(person_at ?p ?ro)) 52 | ) 53 | :effect (and 54 | (at end(robot_near_person ?r ?p)) 55 | ) 56 | ) 57 | 58 | 59 | 60 | );; end Domain ;;;;;;;;;;;;;;;;;;;;;;;; 61 | -------------------------------------------------------------------------------- /plansys2_upf_plan_solver/test/pddl/problem_simple_1.pddl: -------------------------------------------------------------------------------- 1 | (define (problem simple_1) 2 | (:domain simple) 3 | (:objects 4 | leia - robot 5 | Jack - person 6 | kitchen bedroom - room 7 | m1 - message 8 | ) 9 | (:init 10 | (robot_at leia kitchen) 11 | (person_at Jack bedroom) 12 | 13 | 14 | ) 15 | 16 | ;; The goal is to have both packages delivered to their destinations: 17 | (:goal (and 18 | (robot_talk leia m1 Jack) 19 | ) 20 | ) 21 | ) 22 | -------------------------------------------------------------------------------- /plansys2_upf_plan_solver/test/pddl/problem_simple_2.pddl: -------------------------------------------------------------------------------- 1 | (define (problem simple_1) 2 | (:domain simple) 3 | (:objects 4 | leia - robot 5 | Jack - person 6 | kitchen bedroom - room 7 | m1 - message 8 | ) 9 | (:init 10 | (robot_at leia kitchen) 11 | 12 | 13 | ) 14 | 15 | ;; The goal is to have both packages delivered to their destinations: 16 | (:goal (and 17 | (robot_talk leia m1 Jack) 18 | ) 19 | ) 20 | ) 21 | -------------------------------------------------------------------------------- /plansys2_upf_plan_solver/test/pddl/problem_simple_3.pddl: -------------------------------------------------------------------------------- 1 | (define (problem simple_1) 2 | (:domain simple) 3 | (:objects 4 | leia - robot 5 | Jack - person 6 | kitchen bedroom - room 7 | m1 - message 8 | ) 9 | (:init 10 | (robot_at leia kitchen) 11 | ) 12 | 13 | ;; The goal is to have both packages delivered to their destinations: 14 | (:goa (and 15 | (robot_talk leia m1 Jack) 16 | 17 | ) 18 | ) 19 | -------------------------------------------------------------------------------- /plansys2_upf_plan_solver/test/unit/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ament_add_gtest(upf_test upf_test.cpp) 2 | target_link_libraries(upf_test ${PROJECT_NAME} dl) 3 | target_compile_definitions(upf_test PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") 4 | -------------------------------------------------------------------------------- /plansys2_upf_plan_solver/test/unit/upf_test.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2019 Intelligent Robotics Lab 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | 24 | #include "ament_index_cpp/get_package_share_directory.hpp" 25 | 26 | #include "gtest/gtest.h" 27 | #include "plansys2_upf_plan_solver/upf_plan_solver.hpp" 28 | 29 | #include "pluginlib/class_loader.hpp" 30 | #include "pluginlib/class_list_macros.hpp" 31 | #include "plansys2_core/PlanSolverBase.hpp" 32 | 33 | void test_plan_generation(const std::string & argument = "") 34 | { 35 | std::string pkgpath = ament_index_cpp::get_package_share_directory("plansys2_upf_plan_solver"); 36 | std::ifstream domain_ifs(pkgpath + "/pddl/domain_simple.pddl"); 37 | std::string domain_str(( 38 | std::istreambuf_iterator(domain_ifs)), 39 | std::istreambuf_iterator()); 40 | 41 | std::ifstream problem_ifs(pkgpath + "/pddl/problem_simple_1.pddl"); 42 | std::string problem_str(( 43 | std::istreambuf_iterator(problem_ifs)), 44 | std::istreambuf_iterator()); 45 | 46 | auto node = rclcpp_lifecycle::LifecycleNode::make_shared("test_node"); 47 | auto planner = std::make_shared(); 48 | planner->configure(node, "UPF"); 49 | node->set_parameter(rclcpp::Parameter("UPF.arguments", argument)); 50 | 51 | auto plan = planner->getPlan(domain_str, problem_str, "generate_plan_good"); 52 | 53 | ASSERT_TRUE(plan); 54 | ASSERT_EQ(plan.value().items.size(), 3); 55 | 56 | std::vector actions; 57 | for (const auto & action : plan.value().items) { 58 | std::string current = action.action; 59 | std::transform(current.begin(),current.end(),current.begin(), 60 | [](unsigned char c){ return std::tolower(c); }); 61 | actions.push_back(current); 62 | } 63 | 64 | ASSERT_EQ(actions[0], "(move leia kitchen bedroom)"); 65 | ASSERT_EQ(actions[1], "(approach leia bedroom jack)"); 66 | ASSERT_EQ(actions[2], "(talk leia jack jack m1)"); 67 | } 68 | 69 | TEST(upf_plan_solver, generate_plan_good) 70 | { 71 | test_plan_generation(); 72 | } 73 | 74 | TEST(upf_plan_solver, generate_plan_good_with_argument) 75 | { 76 | test_plan_generation("-e"); 77 | } 78 | 79 | TEST(upf_plan_solver, load_upf_plugin) 80 | { 81 | try { 82 | pluginlib::ClassLoader lp_loader( 83 | "plansys2_core", "plansys2::PlanSolverBase"); 84 | plansys2::PlanSolverBase::Ptr plugin = 85 | lp_loader.createUniqueInstance("plansys2/UPFPlanSolver"); 86 | ASSERT_TRUE(true); 87 | } catch (std::exception & e) { 88 | std::cerr << e.what() << std::endl; 89 | ASSERT_TRUE(false); 90 | } 91 | } 92 | 93 | TEST(upf_plan_solver, check_1_ok_domain) 94 | { 95 | std::string pkgpath = ament_index_cpp::get_package_share_directory("plansys2_upf_plan_solver"); 96 | std::ifstream domain_ifs(pkgpath + "/pddl/domain_1_ok.pddl"); 97 | std::string domain_str(( 98 | std::istreambuf_iterator(domain_ifs)), 99 | std::istreambuf_iterator()); 100 | 101 | auto node = rclcpp_lifecycle::LifecycleNode::make_shared("test_node"); 102 | auto planner = std::make_shared(); 103 | planner->configure(node, "UPF"); 104 | 105 | bool result = planner->is_valid_domain(domain_str, "check_1_ok_domain"); 106 | 107 | ASSERT_TRUE(result); 108 | } 109 | 110 | 111 | TEST(upf_plan_solver, check_2_error_domain) 112 | { 113 | std::string pkgpath = ament_index_cpp::get_package_share_directory("plansys2_upf_plan_solver"); 114 | std::ifstream domain_ifs(pkgpath + "/pddl/domain_2_error.pddl"); 115 | std::string domain_str(( 116 | std::istreambuf_iterator(domain_ifs)), 117 | std::istreambuf_iterator()); 118 | 119 | auto node = rclcpp_lifecycle::LifecycleNode::make_shared("test_node"); 120 | auto planner = std::make_shared(); 121 | planner->configure(node, "UPF"); 122 | 123 | bool result = planner->is_valid_domain(domain_str, "check_2_error_domain"); 124 | 125 | ASSERT_FALSE(result); 126 | } 127 | 128 | 129 | TEST(upf_plan_solver, generate_plan_unsolvable) 130 | { 131 | std::string pkgpath = ament_index_cpp::get_package_share_directory("plansys2_upf_plan_solver"); 132 | std::ifstream domain_ifs(pkgpath + "/pddl/domain_simple.pddl"); 133 | std::string domain_str(( 134 | std::istreambuf_iterator(domain_ifs)), 135 | std::istreambuf_iterator()); 136 | 137 | std::ifstream problem_ifs(pkgpath + "/pddl/problem_simple_2.pddl"); 138 | std::string problem_str(( 139 | std::istreambuf_iterator(problem_ifs)), 140 | std::istreambuf_iterator()); 141 | 142 | auto node = rclcpp_lifecycle::LifecycleNode::make_shared("test_node"); 143 | auto planner = std::make_shared(); 144 | planner->configure(node, "UPF"); 145 | 146 | auto plan = planner->getPlan(domain_str, problem_str); 147 | 148 | ASSERT_FALSE(plan); 149 | } 150 | 151 | TEST(upf_plan_solver, generate_plan_error) 152 | { 153 | std::string pkgpath = ament_index_cpp::get_package_share_directory("plansys2_upf_plan_solver"); 154 | std::ifstream domain_ifs(pkgpath + "/pddl/domain_simple.pddl"); 155 | std::string domain_str(( 156 | std::istreambuf_iterator(domain_ifs)), 157 | std::istreambuf_iterator()); 158 | 159 | std::ifstream problem_ifs(pkgpath + "/pddl/problem_simple_3.pddl"); 160 | std::string problem_str(( 161 | std::istreambuf_iterator(problem_ifs)), 162 | std::istreambuf_iterator()); 163 | 164 | auto node = rclcpp_lifecycle::LifecycleNode::make_shared("test_node"); 165 | auto planner = std::make_shared(); 166 | planner->configure(node, "UPF"); 167 | 168 | auto plan = planner->getPlan(domain_str, problem_str); 169 | 170 | ASSERT_FALSE(plan); 171 | } 172 | 173 | int main(int argc, char ** argv) 174 | { 175 | testing::InitGoogleTest(&argc, argv); 176 | rclcpp::init(argc, argv); 177 | 178 | return RUN_ALL_TESTS(); 179 | } 180 | -------------------------------------------------------------------------------- /upf.repos: -------------------------------------------------------------------------------- 1 | repositories: 2 | unified-planning: 3 | type: git 4 | url: https://github.com/aiplan4eu/unified-planning.git 5 | version: master 6 | up-tamer: 7 | type: git 8 | url: https://github.com/aiplan4eu/up-tamer.git 9 | version: master 10 | up-pyperplan: 11 | type: git 12 | url: https://github.com/aiplan4eu/up-pyperplan.git 13 | version: master 14 | ros2_planning_system: 15 | type: git 16 | url: https://github.com/PlanSys2/ros2_planning_system.git 17 | version: humble-devel 18 | cascade_lifecycle: 19 | type: git 20 | url: https://github.com/fmrico/cascade_lifecycle.git 21 | version: humble-devel 22 | -------------------------------------------------------------------------------- /upf4ros2/launch/upf4ros2.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2021 Intelligent Robotics Lab 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from launch import LaunchDescription 16 | from launch_ros.actions import Node 17 | 18 | 19 | def generate_launch_description(): 20 | 21 | upf4ros2_cmd = Node(package='upf4ros2', 22 | executable='upf4ros2_main', 23 | output='screen') 24 | 25 | ld = LaunchDescription() 26 | ld.add_action(upf4ros2_cmd) 27 | 28 | return ld 29 | -------------------------------------------------------------------------------- /upf4ros2/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | upf4ros2 5 | 0.0.0 6 | TODO: Package description 7 | fmrico 8 | Apache License, Version 2.0 9 | 10 | rclpy 11 | sensor_msgs 12 | geometry_msgs 13 | upf_msgs 14 | 15 | ament_copyright 16 | ament_flake8 17 | ament_pep257 18 | python3-pytest 19 | 20 | 21 | ament_python 22 | 23 | 24 | -------------------------------------------------------------------------------- /upf4ros2/resource/upf4ros2: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PlanSys2/UPF4ROS2/77a81582d302da03bdce1f05a95b09a0a5c3cffb/upf4ros2/resource/upf4ros2 -------------------------------------------------------------------------------- /upf4ros2/setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script_dir=$base/lib/upf4ros2 3 | [install] 4 | install_scripts=$base/lib/upf4ros2 5 | -------------------------------------------------------------------------------- /upf4ros2/setup.py: -------------------------------------------------------------------------------- 1 | from glob import glob 2 | import os 3 | 4 | from setuptools import setup 5 | 6 | package_name = 'upf4ros2' 7 | 8 | setup( 9 | name=package_name, 10 | version='0.0.0', 11 | packages=[package_name], 12 | data_files=[ 13 | ('share/ament_index/resource_index/packages', 14 | ['resource/' + package_name]), 15 | ('share/' + package_name, ['package.xml']), 16 | (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')), 17 | (os.path.join('share', package_name, 'pddl'), glob('tests/pddl/*')), 18 | ], 19 | install_requires=['setuptools'], 20 | zip_safe=True, 21 | maintainer='fmrico', 22 | maintainer_email='fmrico@gmail.com', 23 | description='ROS 2 Support for UPF', 24 | license='Apache License, Version 2.0', 25 | tests_require=['pytest'], 26 | entry_points={ 27 | 'console_scripts': [ 28 | 'upf4ros2_main = upf4ros2.upf4ros2_main:main' 29 | ], 30 | }, 31 | ) 32 | -------------------------------------------------------------------------------- /upf4ros2/tests/pddl/blocks_domain.pddl: -------------------------------------------------------------------------------- 1 | (define (domain blocks) 2 | (:requirements :strips :equality) 3 | 4 | (:predicates 5 | (on ?x ?y) 6 | (clear ?x) 7 | (block ?b) 8 | ) 9 | 10 | (:constants Table) 11 | 12 | (:action move 13 | :parameters (?b ?from ?to) 14 | :precondition 15 | (and 16 | (block ?b) 17 | (block ?to) 18 | (clear ?b) 19 | (clear ?to) 20 | (on ?b ?from) 21 | (not (= ?b ?from)) 22 | (not (= ?b ?to)) 23 | (not (= ?from ?to)) 24 | ) 25 | :effect 26 | (and 27 | (on ?b ?to) 28 | (clear ?from) 29 | (not (on ?b ?from)) 30 | (not (clear ?to)) 31 | ) 32 | ) 33 | 34 | (:action move_to_table 35 | :parameters (?b ?x) 36 | :precondition 37 | (and 38 | (block ?b) 39 | (block ?x) 40 | (on ?b ?x) 41 | (clear ?b) 42 | (not (= ?b ?x)) 43 | ) 44 | :effect 45 | (and 46 | (on ?b Table) 47 | (clear ?x) 48 | (not (on ?b ?x)) 49 | ) 50 | ) 51 | ) 52 | 53 | -------------------------------------------------------------------------------- /upf4ros2/tests/pddl/blocks_problem_0.pddl: -------------------------------------------------------------------------------- 1 | (define (problem blocks1) 2 | (:domain blocks) 3 | (:objects 4 | a b c d 5 | ) 6 | (:init 7 | (block a) 8 | (block b) 9 | (block c) 10 | (block d) 11 | (clear d) 12 | (clear b) 13 | (clear c) 14 | (on c Table) 15 | (on d Table) 16 | (on a Table) 17 | (on b a) 18 | ) 19 | 20 | (:goal (and 21 | (on a b)(on b d)(on d c) (on c Table) 22 | ) 23 | ) 24 | 25 | ) 26 | -------------------------------------------------------------------------------- /upf4ros2/tests/pddl/blocks_problem_1.pddl: -------------------------------------------------------------------------------- 1 | (define (problem blocks1) 2 | (:domain blocks) 3 | (:objects 4 | a b c 5 | ) 6 | (:init 7 | (block a) 8 | (block b) 9 | (block c) 10 | (clear b) 11 | (clear c) 12 | (on c Table) 13 | (on a Table) 14 | (on b a) 15 | ) 16 | 17 | (:goal (and 18 | (on b c)) 19 | ) 20 | 21 | ) 22 | -------------------------------------------------------------------------------- /upf4ros2/tests/pddl/domain_tt.pddl: -------------------------------------------------------------------------------- 1 | (define (domain simple) 2 | 3 | (:types robot room) 4 | 5 | (:predicates 6 | (robot_at ?r - robot ?ro - room) 7 | (connected ?r1 ?r2 - room) 8 | 9 | ) 10 | 11 | (:durative-action move 12 | :parameters (?r - robot ?r1 ?r2 - room) 13 | :duration ( = ?duration 5) 14 | :condition (and 15 | (at start(robot_at ?r ?r1)) 16 | (at start(connected ?r1 ?r2))) 17 | :effect (and 18 | (at start(not(robot_at ?r ?r1))) 19 | (at end(robot_at ?r ?r2)) 20 | ) 21 | ) 22 | ) -------------------------------------------------------------------------------- /upf4ros2/tests/pddl/gripper_domain.pddl: -------------------------------------------------------------------------------- 1 | (define (domain gripper-strips) 2 | (:predicates 3 | (room ?r) 4 | (ball ?b) 5 | (gripper ?g) 6 | (at-robby ?r) 7 | (at ?b ?r) 8 | (free ?g) 9 | (carry ?o ?g) 10 | ) 11 | 12 | (:action move 13 | :parameters (?from ?to) 14 | :precondition 15 | (and 16 | (room ?from) 17 | (room ?to) 18 | (at-robby ?from) 19 | ) 20 | :effect 21 | (and 22 | (at-robby ?to) 23 | (not (at-robby ?from))) 24 | ) 25 | 26 | (:action pick 27 | :parameters (?obj ?room ?gripper) 28 | :precondition 29 | (and 30 | (ball ?obj) 31 | (room ?room) 32 | (gripper ?gripper) 33 | (at ?obj ?room) 34 | (at-robby ?room) 35 | (free ?gripper) 36 | ) 37 | :effect 38 | (and 39 | (carry ?obj ?gripper) 40 | (not (at ?obj ?room)) 41 | (not (free ?gripper))) 42 | ) 43 | 44 | (:action drop 45 | :parameters (?obj ?room ?gripper) 46 | :precondition 47 | (and 48 | (ball ?obj) 49 | (room ?room) 50 | (gripper ?gripper) 51 | (carry ?obj ?gripper) 52 | (at-robby ?room) 53 | ) 54 | :effect 55 | (and 56 | (at ?obj ?room) 57 | (free ?gripper) 58 | (not (carry ?obj ?gripper)) 59 | ) 60 | ) 61 | ) 62 | 63 | -------------------------------------------------------------------------------- /upf4ros2/tests/pddl/gripper_problem_0.pddl: -------------------------------------------------------------------------------- 1 | (define (problem strips-gripper2) 2 | (:domain gripper-strips) 3 | (:objects 4 | rooma roomb ball1 ball2 left right 5 | ) 6 | (:init 7 | (room rooma) 8 | (room roomb) 9 | (ball ball1) 10 | (ball ball2) 11 | (gripper left) 12 | (gripper right) 13 | (at-robby rooma) 14 | (free left) 15 | (free right) 16 | (at ball1 rooma) 17 | (at ball2 rooma) 18 | ) 19 | 20 | (:goal (at ball1 roomb)) 21 | 22 | ) 23 | -------------------------------------------------------------------------------- /upf4ros2/tests/pddl/problem_tt_1.pddl: -------------------------------------------------------------------------------- 1 | (define (problem simple_1) 2 | (:domain simple) 3 | (:objects 4 | leia - robot 5 | kitchen bedroom - room 6 | ) 7 | (:init 8 | (robot_at leia kitchen) 9 | (connected kitchen bedroom) 10 | ) 11 | 12 | ;; The goal is to have both packages delivered to their destinations: 13 | (:goal (and 14 | (robot_at leia bedroom) 15 | ) 16 | ) 17 | ) 18 | -------------------------------------------------------------------------------- /upf4ros2/tests/test_conversion.py: -------------------------------------------------------------------------------- 1 | # Copyright 2022 Intelligent Robotics Lab 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | # 15 | 16 | # This module started from the test_protobuf_io.py module from the 17 | # AIPlan4EU project, with the same license 18 | 19 | from itertools import islice 20 | import unittest 21 | 22 | import pytest 23 | 24 | from unified_planning import engines, model, shortcuts 25 | from unified_planning.model import metrics 26 | from unified_planning.plans import ActionInstance 27 | from unified_planning.shortcuts import ( 28 | CompilationKind, 29 | Compiler, 30 | OneshotPlanner, 31 | PlanValidator, 32 | Problem, 33 | UserType 34 | ) 35 | 36 | from unified_planning.test.examples import get_example_problems 37 | 38 | 39 | @pytest.mark.rostest 40 | class TestROS2Interfaces(unittest.TestCase): 41 | 42 | @classmethod 43 | def setUpClass(cls): 44 | shortcuts.get_environment().credits_stream = None 45 | pass 46 | 47 | @classmethod 48 | def tearDownClass(cls): 49 | pass 50 | 51 | def __init__(self, *args, **kwargs): 52 | super().__init__(*args, **kwargs) 53 | 54 | self.problems = get_example_problems() 55 | from upf4ros2.ros2_interface_reader import ROS2InterfaceReader 56 | from upf4ros2.ros2_interface_writer import ROS2InterfaceWriter 57 | 58 | self.pb_writer = ROS2InterfaceWriter() 59 | self.pb_reader = ROS2InterfaceReader() 60 | 61 | def test_fluent(self): 62 | problem = shortcuts.Problem('test') 63 | x = shortcuts.Fluent('x') 64 | 65 | x_pb = self.pb_writer.convert(x, problem) 66 | 67 | self.assertEqual(x_pb.name, 'x') 68 | self.assertEqual(x_pb.value_type, 'up:bool') 69 | 70 | x_up = self.pb_reader.convert(x_pb, problem) 71 | 72 | self.assertEqual(x_up.name, 'x') 73 | self.assertEqual(x_up.type, shortcuts.BoolType()) 74 | 75 | def test_fluent_2(self): 76 | problem = self.problems['robot'].problem 77 | 78 | for f in problem.fluents: 79 | f_pb = self.pb_writer.convert(f, problem) 80 | f_up = self.pb_reader.convert(f_pb, problem) 81 | self.assertEqual(f, f_up) 82 | 83 | def test_fluent_3(self): 84 | """Test to handle subtypes of usertypes of Fluent Expression.""" 85 | problem = self.problems['hierarchical_blocks_world'].problem 86 | 87 | for f in problem.fluents: 88 | f_pb = self.pb_writer.convert(f, problem) 89 | f_up = self.pb_reader.convert(f_pb, problem) 90 | self.assertEqual(f, f_up) 91 | 92 | def test_objects(self): 93 | """Test to handle subtypes of usertypes of Fluent Expression.""" 94 | problem = self.problems['hierarchical_blocks_world'].problem 95 | 96 | for o in problem.all_objects: 97 | o_pb = self.pb_writer.convert(o) 98 | o_up = self.pb_reader.convert(o_pb, problem) 99 | 100 | self.assertEqual(o, o_up) 101 | 102 | def test_expression(self): 103 | problem = Problem('test') 104 | ex = problem.environment.expression_manager.true_expression 105 | 106 | ex_pb = self.pb_writer.convert(ex) 107 | ex_up = self.pb_reader.convert(ex_pb, problem) 108 | self.assertEqual(ex, ex_up) 109 | 110 | ex = problem.environment.expression_manager.Int(10) 111 | 112 | ex_pb = self.pb_writer.convert(ex) 113 | ex_up = self.pb_reader.convert(ex_pb, problem) 114 | self.assertEqual(ex, ex_up) 115 | 116 | def test_fluent_expressions(self): 117 | problem = self.problems['hierarchical_blocks_world'].problem 118 | 119 | problem_pb = self.pb_writer.convert(problem) 120 | problem_up = self.pb_reader.convert(problem_pb) 121 | 122 | self.assertEqual(problem, problem_up) 123 | 124 | def test_type_declaration(self): 125 | problem = Problem('test') 126 | ex = UserType('object') 127 | ex_pb = self.pb_writer.convert(ex) 128 | ex_up = self.pb_reader.convert(ex_pb, problem) 129 | self.assertEqual(ex, ex_up) 130 | 131 | o = model.Object('o', ex) 132 | problem.add_object(o) 133 | 134 | ex = UserType('location', ex) 135 | ex_pb = self.pb_writer.convert(ex) 136 | ex_up = self.pb_reader.convert(ex_pb, problem) 137 | self.assertEqual(ex, ex_up) 138 | 139 | def test_object_declaration(self): 140 | problem = Problem('test') 141 | loc_type = UserType('location') 142 | obj = problem.add_object('l1', loc_type) 143 | obj_pb = self.pb_writer.convert(obj) 144 | obj_up = self.pb_reader.convert(obj_pb, problem) 145 | self.assertEqual(obj, obj_up) 146 | 147 | def test_problem(self): 148 | import unified_planning.grpc.generated.unified_planning_pb2 as up_pb2 149 | 150 | problem = self.problems['robot'].problem 151 | 152 | problem_pb = self.pb_writer.convert(problem) 153 | problem_up = self.pb_reader.convert(problem_pb) 154 | 155 | pb_features = {up_pb2.Feature.Name(feature) 156 | for feature in problem_pb.features} 157 | self.assertEqual(set(problem.kind.features), pb_features) 158 | self.assertEqual(problem, problem_up) 159 | 160 | def test_action(self): 161 | problem = self.problems['robot'].problem 162 | action = problem.action('move') 163 | 164 | action_pb = self.pb_writer.convert(action) 165 | action_up = self.pb_reader.convert(action_pb, problem) 166 | 167 | self.assertEqual(action, action_up) 168 | 169 | def test_durative_action(self): 170 | problem = self.problems['matchcellar'].problem 171 | action = problem.action('mend_fuse') 172 | 173 | action_pb = self.pb_writer.convert(action) 174 | action_up = self.pb_reader.convert(action_pb, problem) 175 | 176 | self.assertEqual(action, action_up) 177 | 178 | def test_action_instance(self): 179 | problem = self.problems['robot'].problem 180 | plan = self.problems['robot'].valid_plans[0] 181 | action_instance = plan.actions[0] 182 | 183 | action_instance_pb = self.pb_writer.convert(action_instance) 184 | action_instance_up = self.pb_reader.convert( 185 | action_instance_pb, problem) 186 | 187 | self.assertEqual(action_instance.action, action_instance_up.action) 188 | self.assertEqual( 189 | action_instance.actual_parameters, 190 | action_instance_up.actual_parameters) 191 | 192 | def test_plan(self): 193 | problem = self.problems['robot'].problem 194 | plan = self.problems['robot'].valid_plans[0] 195 | 196 | plan_pb = self.pb_writer.convert(plan) 197 | plan_up = self.pb_reader.convert(plan_pb, problem) 198 | 199 | self.assertEqual(plan, plan_up) 200 | 201 | def test_time_triggered_plan(self): 202 | problem = self.problems['temporal_conditional'].problem 203 | plan = self.problems['temporal_conditional'].valid_plans[0] 204 | 205 | plan_pb = self.pb_writer.convert(plan) 206 | plan_up = self.pb_reader.convert(plan_pb, problem) 207 | 208 | self.assertEqual(plan, plan_up) 209 | 210 | def test_metric(self): 211 | problem = Problem('test') 212 | problem.add_quality_metric( 213 | metric=metrics.MinimizeSequentialPlanLength()) 214 | problem.add_quality_metric(metric=metrics.MinimizeMakespan()) 215 | problem.add_quality_metric( 216 | metric=metrics.MinimizeExpressionOnFinalState(6) 217 | ) 218 | problem.add_quality_metric( 219 | metric=metrics.MaximizeExpressionOnFinalState(3.5) 220 | ) 221 | 222 | for metric in problem.quality_metrics: 223 | metric_pb = self.pb_writer.convert(metric) 224 | metric_up = self.pb_reader.convert(metric_pb, problem) 225 | 226 | self.assertEqual(str(metric), str(metric_up)) 227 | 228 | def test_log_message(self): 229 | def assert_log(log): 230 | logger_pb = self.pb_writer.convert(log) 231 | logger_up = self.pb_reader.convert(logger_pb) 232 | self.assertEqual(log, logger_up) 233 | 234 | log = engines.LogMessage(engines.LogLevel.DEBUG, 'test message') 235 | assert_log(log) 236 | log = engines.LogMessage(engines.LogLevel.INFO, 'test message') 237 | assert_log(log) 238 | log = engines.LogMessage(engines.LogLevel.WARNING, 'test message') 239 | assert_log(log) 240 | log = engines.LogMessage(engines.LogLevel.ERROR, 'test message') 241 | assert_log(log) 242 | 243 | def test_plan_generation(self): 244 | problem = self.problems['robot'].problem 245 | 246 | with OneshotPlanner(name='tamer', params={'weight': 0.8}) as planner: 247 | self.assertNotEqual(planner, None) 248 | final_report = planner.solve(problem) 249 | 250 | final_report_pb = self.pb_writer.convert(final_report) 251 | final_report_up = self.pb_reader.convert(final_report_pb, problem) 252 | 253 | self.assertEqual(final_report, final_report_up) 254 | 255 | def test_compiler_result(self): 256 | problem = self.problems['hierarchical_blocks_world'].problem 257 | with Compiler(name='up_grounder') as grounder: 258 | ground_result = grounder.compile( 259 | problem, CompilationKind.GROUNDING) 260 | 261 | ground_result_pb = self.pb_writer.convert(ground_result) 262 | ground_result_up = self.pb_reader.convert( 263 | ground_result_pb, problem) 264 | 265 | self.assertEqual(ground_result.problem, ground_result_up.problem) 266 | for grounded_action in ground_result.problem.actions: 267 | # Test both callable 'map_back_action_instance' act 268 | # the same on every action of the grounded_problem 269 | grounded_action_instance = ActionInstance(grounded_action) 270 | original_action_instance_up = ground_result.map_back_action_instance( 271 | grounded_action_instance) 272 | original_action_instance_pb = ground_result_up.map_back_action_instance( 273 | grounded_action_instance) 274 | self.assertEqual( 275 | original_action_instance_pb.action, 276 | original_action_instance_up.action, 277 | ) 278 | self.assertEqual( 279 | original_action_instance_pb.actual_parameters, 280 | original_action_instance_up.actual_parameters, 281 | ) 282 | 283 | def test_validation_result(self): 284 | problem = self.problems['robot'].problem 285 | 286 | with OneshotPlanner(name='tamer', params={'weight': 0.8}) as planner: 287 | self.assertNotEqual(planner, None) 288 | final_report = planner.solve(problem) 289 | with PlanValidator(name='tamer') as validator: 290 | validation_result = validator.validate( 291 | problem, final_report.plan) 292 | # prueba = self.pb_writer._convert_validation_result(validation_result) 293 | validation_result_pb = self.pb_writer.convert( 294 | validation_result) 295 | validation_result_up = self.pb_reader.convert( 296 | validation_result_pb) 297 | 298 | self.assertEqual( 299 | validation_result.metrics, 300 | validation_result_up.metrics) 301 | self.assertEqual( 302 | validation_result.trace, 303 | validation_result_up.trace) 304 | self.assertEqual(validation_result, validation_result_up) 305 | 306 | 307 | class TestROS2InterfacesProblems(unittest.TestCase): 308 | 309 | def __init__(self, *args, **kwargs): 310 | super().__init__(*args, **kwargs) 311 | 312 | self.problems = get_example_problems() 313 | from upf4ros2.ros2_interface_reader import ROS2InterfaceReader 314 | from upf4ros2.ros2_interface_writer import ROS2InterfaceWriter 315 | 316 | self.pb_writer = ROS2InterfaceWriter() 317 | self.pb_reader = ROS2InterfaceReader() 318 | 319 | def test_all_problems(self): 320 | for name, example in islice(self.problems.items(), 10): 321 | problem = example.problem 322 | 323 | problem_pb = self.pb_writer.convert(problem) 324 | problem_up = self.pb_reader.convert(problem_pb) 325 | # self.assertEqual(problem, problem_up) 326 | self.assertEqual(hash(problem), hash(problem_up)) 327 | 328 | def test_all_plans(self): 329 | for name, example in self.problems.items(): 330 | problem = example.problem 331 | if example.valid_plans: 332 | plan = example.valid_plans[0] 333 | plan_pb = self.pb_writer.convert(plan) 334 | plan_up = self.pb_reader.convert(plan_pb, problem) 335 | self.assertEqual(hash(plan), hash(plan_up)) 336 | 337 | def test_some_plan_generations(self): 338 | problems = [ 339 | 'basic', 340 | 'basic_without_negative_preconditions', 341 | 'basic_nested_conjunctions', 342 | 'robot_loader', 343 | 'robot_loader_mod', 344 | 'robot_loader_adv', 345 | 'robot_real_constants', 346 | 'robot_int_battery', 347 | 'robot', 348 | ] 349 | for name in problems: 350 | problem = self.problems[name].problem 351 | 352 | with shortcuts.OneshotPlanner(name='tamer') as planner: 353 | self.assertNotEqual(planner, None) 354 | final_report = planner.solve(problem) 355 | 356 | final_report_pb = self.pb_writer.convert(final_report) 357 | final_report_up = self.pb_reader.convert( 358 | final_report_pb, problem) 359 | 360 | self.assertEqual(final_report.status, final_report_up.status) 361 | self.assertEqual(final_report.plan, final_report_up.plan) 362 | self.assertEqual( 363 | final_report.engine_name, 364 | final_report_up.engine_name) 365 | 366 | 367 | if __name__ == '__main__': 368 | unittest.main() 369 | -------------------------------------------------------------------------------- /upf4ros2/tests/test_copyright.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_copyright.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.copyright 20 | @pytest.mark.linter 21 | def test_copyright(): 22 | rc = main(argv=['.', 'test']) 23 | assert rc == 0, 'Found errors' 24 | -------------------------------------------------------------------------------- /upf4ros2/tests/test_flake8.py: -------------------------------------------------------------------------------- 1 | # Copyright 2017 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_flake8.main import main_with_errors 16 | import pytest 17 | 18 | 19 | @pytest.mark.flake8 20 | @pytest.mark.linter 21 | def test_flake8(): 22 | rc, errors = main_with_errors(argv=[]) 23 | assert rc == 0, \ 24 | 'Found %d code style errors / warnings:\n' % len(errors) + \ 25 | '\n'.join(errors) 26 | -------------------------------------------------------------------------------- /upf4ros2/tests/test_pep257.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_pep257.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.linter 20 | @pytest.mark.pep257 21 | def test_pep257(): 22 | rc = main(argv=['.', 'test']) 23 | assert rc == 0, 'Found code style errors / warnings' 24 | -------------------------------------------------------------------------------- /upf4ros2/tests/test_upf4ros2.py: -------------------------------------------------------------------------------- 1 | # Copyright 2022 Intelligent Robotics Lab 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | # 15 | 16 | import unittest 17 | import threading 18 | import rclpy 19 | 20 | from ament_index_python.packages import get_package_share_directory 21 | 22 | from rclpy.action import ActionClient 23 | from rclpy.executors import SingleThreadedExecutor 24 | 25 | from unified_planning import model 26 | from unified_planning import shortcuts 27 | from unified_planning.test.examples import get_example_problems 28 | from unified_planning.io.pddl_reader import PDDLReader 29 | 30 | from upf4ros2.ros2_interface_reader import ROS2InterfaceReader 31 | from upf4ros2.ros2_interface_writer import ROS2InterfaceWriter 32 | 33 | from upf4ros2 import upf4ros2_main 34 | from upf_msgs import msg as msgs 35 | from upf_msgs.srv import ( 36 | NewProblem, 37 | SetProblem, 38 | GetProblem, 39 | AddFluent, 40 | SetInitialValue, 41 | AddGoal, 42 | AddAction, 43 | AddObject 44 | ) 45 | from upf_msgs.srv import PDDLPlanOneShot as PDDLPlanOneShotSrv 46 | from upf_msgs.action import ( 47 | PDDLPlanOneShot, 48 | PlanOneShot, 49 | ) 50 | 51 | 52 | class TestUPF4ROS2(unittest.TestCase): 53 | 54 | @classmethod 55 | def setUpClass(cls): 56 | rclpy.init() 57 | 58 | @classmethod 59 | def tearDownClass(cls): 60 | rclpy.shutdown() 61 | 62 | def setUp(self): 63 | self.node = rclpy.create_node('test_upf4ros2') 64 | self.node_main = upf4ros2_main.UPF4ROS2Node() 65 | self.executor = SingleThreadedExecutor() 66 | self.executor.add_node(self.node_main) 67 | self.spin_thread = threading.Thread( 68 | target=self.executor.spin, daemon=True) 69 | self.spin_thread.start() 70 | 71 | def tearDown(self): 72 | self.executor.shutdown() 73 | self.node.destroy_node() 74 | self.node_main.destroy_node() 75 | self.spin_thread.join() 76 | 77 | def test_plan_from_file_pddl_no_tt(self): 78 | goal_msg = PDDLPlanOneShot.Goal() 79 | goal_msg.plan_request.mode = msgs.PDDLPlanRequest.FILE 80 | goal_msg.plan_request.domain = (get_package_share_directory('upf4ros2') 81 | + '/pddl/gripper_domain.pddl') 82 | goal_msg.plan_request.problem = ( 83 | get_package_share_directory('upf4ros2') + 84 | '/pddl/gripper_problem_0.pddl') 85 | 86 | reader = PDDLReader() 87 | upf_problem = reader.parse_problem( 88 | goal_msg.plan_request.domain, 89 | goal_msg.plan_request.problem) 90 | 91 | client = ActionClient( 92 | self.node, 93 | PDDLPlanOneShot, 94 | 'upf4ros2/action/planOneShotPDDL') 95 | 96 | def goal_response_callback(future): 97 | goal_handle = future.result() 98 | self.assertTrue(goal_handle.accepted) 99 | if not goal_handle.accepted: 100 | self.node.get_logger().info('Goal rejected :(') 101 | return 102 | 103 | self.node.get_logger().info('Goal accepted :)') 104 | 105 | self.node._get_result_future = goal_handle.get_result_async() 106 | self.node._get_result_future.add_done_callback(get_result_callback) 107 | 108 | def get_result_callback(future): 109 | result = future.result().result 110 | self.assertEqual(result.success, True) 111 | self.assertEqual(result.message, '') 112 | 113 | self.node.get_logger().info('Result: success: {0} message:{1}'. 114 | format(result.success, result.message)) 115 | 116 | def feedback_callback(feedback_msg): 117 | feedback = feedback_msg.feedback 118 | pb_reader = ROS2InterfaceReader() 119 | upf_plan = pb_reader.convert( 120 | feedback.plan_result.plan, upf_problem) 121 | self.node.get_logger().info('Received feedback: {0}'. 122 | format(upf_plan)) 123 | 124 | good_plan = '[pick(ball1, rooma, left), move(rooma, roomb), drop(ball1, roomb, left)]' 125 | self.assertEqual(upf_plan.__repr__(), good_plan) 126 | 127 | client.wait_for_server() 128 | 129 | send_goal_future = client.send_goal_async( 130 | goal_msg, feedback_callback=feedback_callback) 131 | send_goal_future.add_done_callback(goal_response_callback) 132 | 133 | def test_plan_from_file_pddl_tt(self): 134 | goal_msg = PDDLPlanOneShot.Goal() 135 | goal_msg.plan_request.mode = msgs.PDDLPlanRequest.FILE 136 | goal_msg.plan_request.domain = (get_package_share_directory('upf4ros2') 137 | + '/pddl/domain_tt.pddl') 138 | goal_msg.plan_request.problem = ( 139 | get_package_share_directory('upf4ros2') + 140 | '/pddl/problem_tt_1.pddl') 141 | 142 | reader = PDDLReader() 143 | upf_problem = reader.parse_problem( 144 | goal_msg.plan_request.domain, 145 | goal_msg.plan_request.problem) 146 | 147 | client = ActionClient( 148 | self.node, 149 | PDDLPlanOneShot, 150 | 'upf4ros2/action/planOneShotPDDL') 151 | 152 | def goal_response_callback(future): 153 | goal_handle = future.result() 154 | self.assertTrue(goal_handle.accepted) 155 | if not goal_handle.accepted: 156 | self.node.get_logger().info('Goal rejected :(') 157 | return 158 | 159 | self.node.get_logger().info('Goal accepted :)') 160 | 161 | self.node._get_result_future = goal_handle.get_result_async() 162 | self.node._get_result_future.add_done_callback(get_result_callback) 163 | 164 | def get_result_callback(future): 165 | result = future.result().result 166 | self.assertEqual(result.success, True) 167 | self.assertEqual(result.message, '') 168 | 169 | self.node.get_logger().info('Result: success: {0} message:{1}'. 170 | format(result.success, result.message)) 171 | rclpy.shutdown() 172 | 173 | def feedback_callback(feedback_msg): 174 | feedback = feedback_msg.feedback 175 | pb_reader = ROS2InterfaceReader() 176 | upf_plan = pb_reader.convert( 177 | feedback.plan_result.plan, upf_problem) 178 | self.node.get_logger().info('Received feedback: {0}'. 179 | format(upf_plan)) 180 | 181 | good_plan = '[(Fraction(0, 1), move(leia, kitchen, bedroom), Fraction(5, 1))]' 182 | self.assertEqual(upf_plan.__repr__(), good_plan) 183 | 184 | client.wait_for_server() 185 | 186 | send_goal_future = client.send_goal_async( 187 | goal_msg, feedback_callback=feedback_callback) 188 | send_goal_future.add_done_callback(goal_response_callback) 189 | 190 | def test_plan_robot(self): 191 | problems = get_example_problems() 192 | problem = problems['robot'].problem 193 | 194 | pb_writter = ROS2InterfaceWriter() 195 | 196 | goal_msg = PlanOneShot.Goal() 197 | goal_msg.plan_request.problem = pb_writter.convert(problem) 198 | 199 | client = ActionClient( 200 | self.node, 201 | PlanOneShot, 202 | 'upf4ros2/action/planOneShot') 203 | 204 | def goal_response_callback(future): 205 | goal_handle = future.result() 206 | self.assertTrue(goal_handle.accepted) 207 | if not goal_handle.accepted: 208 | self.node.get_logger().info('Goal rejected :(') 209 | return 210 | 211 | self.node.get_logger().info('Goal accepted :)') 212 | 213 | self.node._get_result_future = goal_handle.get_result_async() 214 | self.node._get_result_future.add_done_callback(get_result_callback) 215 | 216 | def get_result_callback(future): 217 | result = future.result().result 218 | self.assertEqual(result.success, True) 219 | self.assertEqual(result.message, '') 220 | 221 | self.node.get_logger().info('Result: success: {0} message:{1}'. 222 | format(result.success, result.message)) 223 | rclpy.shutdown() 224 | 225 | def feedback_callback(feedback_msg): 226 | feedback = feedback_msg.feedback 227 | pb_reader = ROS2InterfaceReader() 228 | upf_plan = pb_reader.convert(feedback.plan_result.plan, problem) 229 | self.node.get_logger().info('Received feedback: {0}'. 230 | format(upf_plan)) 231 | 232 | good_plan = '[move(l1, l2)]' 233 | self.assertEqual(upf_plan.__repr__(), good_plan) 234 | 235 | client.wait_for_server() 236 | 237 | send_goal_future = client.send_goal_async( 238 | goal_msg, feedback_callback=feedback_callback) 239 | send_goal_future.add_done_callback(goal_response_callback) 240 | 241 | def test_plan_from_file_pddl_tt_service(self): 242 | client = self.node.create_client( 243 | PDDLPlanOneShotSrv, 'upf4ros2/srv/planOneShotPDDL') 244 | while not client.wait_for_service(timeout_sec=1.0): 245 | self.node.get_logger().info('service not available, waiting again...') 246 | 247 | srv = PDDLPlanOneShotSrv.Request() 248 | srv.plan_request.mode = msgs.PDDLPlanRequest.FILE 249 | srv.plan_request.domain = (get_package_share_directory('upf4ros2') 250 | + '/pddl/domain_tt.pddl') 251 | srv.plan_request.problem = (get_package_share_directory('upf4ros2') 252 | + '/pddl/problem_tt_1.pddl') 253 | 254 | reader = PDDLReader() 255 | upf_problem = reader.parse_problem( 256 | srv.plan_request.domain, 257 | srv.plan_request.problem) 258 | 259 | future = client.call_async(srv) 260 | rclpy.spin_until_future_complete(self.node, future) 261 | response = future.result() 262 | 263 | pb_reader = ROS2InterfaceReader() 264 | upf_plan = pb_reader.convert(response.plan_result.plan, upf_problem) 265 | self.node.get_logger().info('Received feedback: {0}'.format(upf_plan)) 266 | 267 | good_plan = 'TimeTriggeredPlan([' \ 268 | '(Fraction(0, 1), move(leia, kitchen, bedroom), Fraction(5, 1))' \ 269 | '])' 270 | self.assertEqual(upf_plan.__repr__(), good_plan) 271 | self.assertTrue(response.success) 272 | self.assertEqual(response.message, '') 273 | 274 | def test_new_problem(self): 275 | client = self.node.create_client( 276 | NewProblem, '/upf4ros2/srv/new_problem') 277 | while not client.wait_for_service(timeout_sec=1.0): 278 | self.node.get_logger().info('Waiting for /upf4ros2/srv/new_problem...') 279 | request = NewProblem.Request() 280 | request.problem_name = 'problem_test_1' 281 | future = client.call_async(request) 282 | rclpy.spin_until_future_complete(self.node, future) 283 | response = future.result() 284 | 285 | self.assertIsNotNone(response, 'No se recibió respuesta del servicio') 286 | self.assertTrue( 287 | response.success, 288 | f"Respuesta fallida: {response.message}") 289 | self.assertEqual( 290 | response.message, 291 | '', 292 | "El mensaje de respuesta no es el esperado") 293 | 294 | def test_set_get_problem(self): 295 | 296 | client = self.node.create_client( 297 | SetProblem, 'upf4ros2/srv/set_problem') 298 | while not client.wait_for_service(timeout_sec=1.0): 299 | self.node.get_logger().info('service not available, waiting again...') 300 | 301 | pb_writer = ROS2InterfaceWriter() 302 | 303 | problems = get_example_problems() 304 | problem = problems['robot'].problem 305 | srv = SetProblem.Request() 306 | srv.problem_name = 'problem_test_robot' 307 | srv.problem = pb_writer.convert(problem) 308 | 309 | future = client.call_async(srv) 310 | rclpy.spin_until_future_complete(self.node, future) 311 | response = future.result() 312 | self.assertTrue(response.success) 313 | self.assertEqual(response.message, '') 314 | 315 | def test_add_set_fluent(self): 316 | client = self.node.create_client( 317 | SetProblem, 'upf4ros2/srv/set_problem') 318 | while not client.wait_for_service(timeout_sec=1.0): 319 | self.node.get_logger().info('service not available, waiting again...') 320 | 321 | pb_writer = ROS2InterfaceWriter() 322 | 323 | problems = get_example_problems() 324 | problem = problems['robot'].problem 325 | srv = SetProblem.Request() 326 | srv.problem_name = 'problem_test_robot' 327 | srv.problem = pb_writer.convert(problem) 328 | 329 | future = client.call_async(srv) 330 | rclpy.spin_until_future_complete(self.node, future) 331 | response = future.result() 332 | 333 | self.assertTrue(response.success) 334 | self.assertEqual(response.message, '') 335 | 336 | add_fluent_cli = self.node.create_client( 337 | AddFluent, 'upf4ros2/srv/add_fluent') 338 | 339 | Location = shortcuts.UserType('Location') 340 | robot_at = model.Fluent( 341 | 'robot_at_bis', shortcuts.BoolType(), 342 | l=Location) 343 | 344 | add_fluent_srv = AddFluent.Request() 345 | add_fluent_srv.problem_name = 'problem_test_robot' 346 | add_fluent_srv.fluent = pb_writer.convert(robot_at, problem) 347 | 348 | item = msgs.ExpressionItem() 349 | item.atom.append(msgs.Atom()) 350 | item.atom[0].boolean_atom.append(False) 351 | item.type = 'up:bool' 352 | item.kind = msgs.ExpressionItem.CONSTANT 353 | value = msgs.Expression() 354 | value.expressions.append(item) 355 | value.level.append(0) 356 | 357 | add_fluent_srv.default_value = value 358 | 359 | future = add_fluent_cli.call_async(add_fluent_srv) 360 | rclpy.spin_until_future_complete(self.node, future) 361 | add_fluent_response = future.result() 362 | 363 | self.assertTrue(add_fluent_response.success) 364 | self.assertEqual(add_fluent_response.message, '') 365 | 366 | problem.add_fluent(robot_at, default_initial_value=False) 367 | 368 | set_initial_value_cli = self.node.create_client( 369 | SetInitialValue, 'upf4ros2/srv/set_initial_value') 370 | set_initial_value_srv = SetInitialValue.Request() 371 | set_initial_value_srv.problem_name = 'problem_test_robot' 372 | l2 = model.Object('l2', Location) 373 | set_initial_value_srv.expression = pb_writer.convert(robot_at(l2)) 374 | set_initial_value_srv.value = value 375 | 376 | future = set_initial_value_cli.call_async(set_initial_value_srv) 377 | rclpy.spin_until_future_complete(self.node, future) 378 | set_initial_value_response = future.result() 379 | self.assertTrue(set_initial_value_response.success) 380 | self.assertEqual(set_initial_value_response.message, '') 381 | 382 | problem.set_initial_value(robot_at(l2), False) 383 | 384 | add_goal_cli = self.node.create_client(AddGoal, 'upf4ros2/add_goal') 385 | add_goal_srv = AddGoal.Request() 386 | add_goal_srv.problem_name = 'problem_test_robot' 387 | l1 = model.Object('l1', Location) 388 | add_goal_srv.goal.append(msgs.Goal()) 389 | add_goal_srv.goal[0].goal = pb_writer.convert(robot_at(l1)) 390 | 391 | future = add_goal_cli.call_async(add_goal_srv) 392 | rclpy.spin_until_future_complete(self.node, future) 393 | add_goal_response = future.result() 394 | self.assertTrue(add_goal_response.success) 395 | self.assertEqual(add_goal_response.message, '') 396 | 397 | problem.add_goal(robot_at(l1)) 398 | 399 | ############################################################### 400 | 401 | client2 = self.node.create_client( 402 | GetProblem, 'upf4ros2/srv/get_problem') 403 | while not client.wait_for_service(timeout_sec=1.0): 404 | self.node.get_logger().info('service not available, waiting again...') 405 | 406 | pb_reader = ROS2InterfaceReader() 407 | 408 | srv2 = GetProblem.Request() 409 | srv2.problem_name = 'problem_test_robot' 410 | 411 | future = client2.call_async(srv2) 412 | rclpy.spin_until_future_complete(self.node, future) 413 | response2 = future.result() 414 | self.assertTrue(response2.success) 415 | 416 | problem_ret = pb_reader.convert(response2.problem) 417 | 418 | self.assertEqual(problem, problem_ret) 419 | 420 | def test_add_action(self): 421 | client = self.node.create_client( 422 | SetProblem, 'upf4ros2/srv/set_problem') 423 | while not client.wait_for_service(timeout_sec=1.0): 424 | self.node.get_logger().info('service not available, waiting again...') 425 | 426 | pb_writer = ROS2InterfaceWriter() 427 | 428 | problems = get_example_problems() 429 | problem = problems['robot'].problem 430 | srv = SetProblem.Request() 431 | srv.problem_name = 'problem_test_robot' 432 | srv.problem = pb_writer.convert(problem) 433 | 434 | future = client.call_async(srv) 435 | rclpy.spin_until_future_complete(self.node, future) 436 | response = future.result() 437 | self.assertTrue(response.success) 438 | self.assertEqual(response.message, '') 439 | 440 | add_action_cli = self.node.create_client( 441 | AddAction, 'upf4ros2/srv/add_action') 442 | 443 | Location = shortcuts.UserType('Location') 444 | robot_at = model.Fluent('robot_at', shortcuts.BoolType(), l=Location) 445 | 446 | move = model.InstantaneousAction( 447 | 'move2', l_from=Location, l_to=Location) 448 | l_from = move.parameter('l_from') 449 | l_to = move.parameter('l_to') 450 | move.add_precondition(robot_at(l_from)) 451 | move.add_effect(robot_at(l_from), False) 452 | move.add_effect(robot_at(l_to), True) 453 | 454 | add_action_srv = AddAction.Request() 455 | add_action_srv.problem_name = 'problem_test_robot' 456 | add_action_srv.action = pb_writer.convert(move) 457 | 458 | future = add_action_cli.call_async(add_action_srv) 459 | rclpy.spin_until_future_complete(self.node, future) 460 | add_action_response = future.result() 461 | self.assertTrue(add_action_response.success) 462 | self.assertEqual(add_action_response.message, '') 463 | 464 | def test_add_object(self): 465 | client = self.node.create_client( 466 | SetProblem, 'upf4ros2/srv/set_problem') 467 | while not client.wait_for_service(timeout_sec=1.0): 468 | self.node.get_logger().info('service not available, waiting again...') 469 | 470 | pb_writer = ROS2InterfaceWriter() 471 | 472 | problems = get_example_problems() 473 | problem = problems['robot'].problem 474 | srv = SetProblem.Request() 475 | srv.problem_name = 'problem_test_robot' 476 | srv.problem = pb_writer.convert(problem) 477 | 478 | future = client.call_async(srv) 479 | rclpy.spin_until_future_complete(self.node, future) 480 | response = future.result() 481 | self.assertTrue(response.success) 482 | self.assertEqual(response.message, '') 483 | 484 | add_object_cli = self.node.create_client( 485 | AddObject, 'upf4ros2/srv/add_object') 486 | 487 | Location = shortcuts.UserType('Location') 488 | 489 | upf_object = model.Object('l3', Location) 490 | 491 | add_object_srv = AddObject.Request() 492 | add_object_srv.problem_name = 'problem_test_robot' 493 | add_object_srv.object = pb_writer.convert(upf_object) 494 | 495 | future = add_object_cli.call_async(add_object_srv) 496 | rclpy.spin_until_future_complete(self.node, future) 497 | add_object_response = future.result() 498 | self.assertTrue(add_object_response.success) 499 | self.assertEqual(add_object_response.message, '') 500 | 501 | problem.add_object(upf_object) 502 | 503 | 504 | if __name__ == '__main__': 505 | unittest.main() 506 | -------------------------------------------------------------------------------- /upf4ros2/upf4ros2/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PlanSys2/UPF4ROS2/77a81582d302da03bdce1f05a95b09a0a5c3cffb/upf4ros2/upf4ros2/__init__.py -------------------------------------------------------------------------------- /upf4ros2/upf4ros2/converter.py: -------------------------------------------------------------------------------- 1 | # Copyright 2022 Intelligent Robotics Lab 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | 16 | class handles: 17 | 18 | def __init__(self, *what): 19 | self.what = what 20 | 21 | def __call__(self, func): 22 | func._what = self.what 23 | return func 24 | 25 | 26 | class Converter: 27 | 28 | def __init__(self): 29 | self.functions = {} 30 | for k in dir(self): 31 | v = getattr(self, k) 32 | if hasattr(v, '_what'): 33 | for x in v._what: 34 | self.functions[x] = v 35 | 36 | def convert(self, element, *args): 37 | f = self.functions[type(element)] 38 | return f(element, *args) 39 | -------------------------------------------------------------------------------- /upf4ros2/upf4ros2/ros2_utils.py: -------------------------------------------------------------------------------- 1 | # Copyright 2022 Intelligent Robotics Lab 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | 16 | from upf_msgs import msg as msgs 17 | 18 | kind_to_string = { 19 | msgs.ExpressionItem.UNKNOWN: 'UNKNOWN', 20 | msgs.ExpressionItem.CONSTANT: 'CONSTANT', 21 | msgs.ExpressionItem.PARAMETER: 'PARAMETER', 22 | msgs.ExpressionItem.VARIABLE: 'VARIABLE', 23 | msgs.ExpressionItem.FLUENT_SYMBOL: 'FLUENT_SYMBOL', 24 | msgs.ExpressionItem.FUNCTION_SYMBOL: 'FUNCTION_SYMBOL', 25 | msgs.ExpressionItem.STATE_VARIABLE: 'STATE_VARIABLE', 26 | msgs.ExpressionItem.FUNCTION_APPLICATION: 'FUNCTION_APPLICATION', 27 | msgs.ExpressionItem.CONTAINER_ID: 'CONTAINER_ID', 28 | } 29 | 30 | 31 | def print_expr(expr: msgs.Expression): 32 | for i in range(len(expr.expressions)): 33 | print( 34 | f'{expr.level[i] * " "} ' 35 | f'[{kind_to_string[expr.expressions[i].kind]}] {expr.expressions[i]}') 36 | -------------------------------------------------------------------------------- /upf4ros2/upf4ros2/upf4ros2_main.py: -------------------------------------------------------------------------------- 1 | # Copyright 2022 Intelligent Robotics Lab 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | # 15 | 16 | import tempfile 17 | 18 | import rclpy 19 | from rclpy.action import ActionServer 20 | from rclpy.node import Node 21 | 22 | from unified_planning import model 23 | from unified_planning.io.pddl_reader import PDDLReader 24 | from unified_planning.shortcuts import OneshotPlanner 25 | 26 | from upf4ros2.ros2_interface_reader import ROS2InterfaceReader 27 | from upf4ros2.ros2_interface_writer import ROS2InterfaceWriter 28 | 29 | from upf_msgs.action import ( 30 | PDDLPlanOneShot, 31 | PlanOneShot 32 | ) 33 | 34 | from upf_msgs.msg import ( 35 | PDDLPlanRequest 36 | ) 37 | 38 | from upf_msgs.srv import ( 39 | AddAction, 40 | AddFluent, 41 | AddGoal, 42 | AddObject, 43 | GetProblem, 44 | NewProblem, 45 | SetInitialValue, 46 | SetProblem 47 | ) 48 | 49 | from upf_msgs.srv import PDDLPlanOneShot as PDDLPlanOneShotSrv 50 | from upf_msgs.srv import PlanOneShot as PlanOneShotSrv 51 | 52 | 53 | class UPF4ROS2Node(Node): 54 | 55 | def __init__(self): 56 | super().__init__('upf4ros2') 57 | 58 | self.problems = {} 59 | 60 | self._ros2_interface_writer = ROS2InterfaceWriter() 61 | self._ros2_interface_reader = ROS2InterfaceReader() 62 | self._pddl_plan_one_shot_server = ActionServer( 63 | self, 64 | PDDLPlanOneShot, 65 | 'upf4ros2/action/planOneShotPDDL', 66 | self.pddl_plan_one_shot_callback) 67 | 68 | self._plan_one_shot_server = ActionServer( 69 | self, 70 | PlanOneShot, 71 | 'upf4ros2/action/planOneShot', 72 | self.plan_one_shot_callback) 73 | 74 | self._get_problem = self.create_service( 75 | GetProblem, 'upf4ros2/srv/get_problem', self.get_problem) 76 | self._new_problem = self.create_service( 77 | NewProblem, 'upf4ros2/srv/new_problem', self.new_problem) 78 | self._set_problem = self.create_service( 79 | SetProblem, 'upf4ros2/srv/set_problem', self.set_problem) 80 | self._add_fluent = self.create_service( 81 | AddFluent, 'upf4ros2/srv/add_fluent', self.add_fluent) 82 | self._add_action = self.create_service( 83 | AddAction, 'upf4ros2/srv/add_action', self.add_action) 84 | self._add_object = self.create_service( 85 | AddObject, 'upf4ros2/srv/add_object', self.add_object) 86 | self._set_initial_value = self.create_service( 87 | SetInitialValue, 'upf4ros2/srv/set_initial_value', self.set_initial_value) 88 | self._add_goal = self.create_service( 89 | AddGoal, 'upf4ros2/add_goal', self.add_goal) 90 | self._pddl_plan_one_shot_srv = self.create_service( 91 | PDDLPlanOneShotSrv, 'upf4ros2/srv/planOneShotPDDL', self.pddl_plan_one_shot) 92 | self._plan_one_shot_srv = self.create_service( 93 | PlanOneShotSrv, 'upf4ros2/srv/planOneShot', self.plan_one_shot) 94 | 95 | def get_problem(self, request, response): 96 | if request.problem_name not in self.problems: 97 | response.success = False 98 | response.message = f'Problem {request.problem_name} does not exist' 99 | else: 100 | response.problem = self._ros2_interface_writer.convert( 101 | self.problems[request.problem_name]) 102 | response.success = True 103 | return response 104 | 105 | def new_problem(self, request, response): 106 | if request.problem_name in self.problems: 107 | response.success = False 108 | response.message = f'Problem {request.problem_name} already exists' 109 | else: 110 | self.problems[request.problem_name] = model.Problem( 111 | request.problem_name) 112 | response.success = True 113 | return response 114 | 115 | def set_problem(self, request, response): 116 | if request.problem_name in self.problems: 117 | response.success = False 118 | response.message = f'Problem {request.problem_name} already exists' 119 | else: 120 | self.problems[request.problem_name] = \ 121 | self._ros2_interface_reader.convert(request.problem) 122 | response.success = True 123 | return response 124 | 125 | def add_fluent(self, request, response): 126 | if request.problem_name not in self.problems: 127 | response.success = False 128 | response.message = f'Problem {request.problem_name} does not exist' 129 | else: 130 | problem = self.problems[request.problem_name] 131 | 132 | fluent = self._ros2_interface_reader.convert( 133 | request.fluent, problem) 134 | value = self._ros2_interface_reader.convert( 135 | request.default_value, problem) 136 | problem.add_fluent(fluent, default_initial_value=value) 137 | response.success = True 138 | return response 139 | 140 | def add_action(self, request, response): 141 | if request.problem_name not in self.problems: 142 | response.success = False 143 | response.message = f'Problem {request.problem_name} does not exist' 144 | else: 145 | problem = self.problems[request.problem_name] 146 | 147 | action = self._ros2_interface_reader.convert( 148 | request.action, problem) 149 | problem.add_action(action) 150 | response.success = True 151 | return response 152 | 153 | def add_object(self, request, response): 154 | if request.problem_name not in self.problems: 155 | response.success = False 156 | response.message = f'Problem {request.problem_name} does not exist' 157 | else: 158 | problem = self.problems[request.problem_name] 159 | 160 | action = self._ros2_interface_reader.convert( 161 | request.object, problem) 162 | problem.add_object(action) 163 | response.success = True 164 | 165 | return response 166 | 167 | def set_initial_value(self, request, response): 168 | if request.problem_name not in self.problems: 169 | response.success = False 170 | response.message = f'Problem {request.problem_name} does not exist' 171 | else: 172 | problem = self.problems[request.problem_name] 173 | 174 | expression = self._ros2_interface_reader.convert( 175 | request.expression, problem) 176 | value = self._ros2_interface_reader.convert(request.value, problem) 177 | problem.set_initial_value(expression, value) 178 | response.success = True 179 | return response 180 | 181 | def add_goal(self, request, response): 182 | if request.problem_name not in self.problems: 183 | response.success = False 184 | response.message = f'Problem {request.problem_name} does not exist' 185 | else: 186 | problem = self.problems[request.problem_name] 187 | 188 | if len(request.goal) > 0: 189 | goal = self._ros2_interface_reader.convert( 190 | request.goal[0].goal, problem) 191 | problem.add_goal(goal) 192 | response.success = True 193 | elif len(request.goal_with_cost) > 0: 194 | goal = self._ros2_interface_reader.convert( 195 | request.goal_with_cost[0].goal, problem) 196 | problem.add_goal(goal) 197 | response.success = True 198 | else: 199 | response.success = False 200 | response.message = 'Goal is void' 201 | return response 202 | 203 | def plan_one_shot(self, request, response): 204 | 205 | upf_problem = self._ros2_interface_reader.convert(request.problem) 206 | 207 | with OneshotPlanner(problem_kind=upf_problem.kind) as planner: 208 | result = planner.solve(upf_problem) 209 | print('%s returned: %s' % (planner.name, result.plan)) 210 | 211 | if result.plan is not None: 212 | response.plan_result = self._ros2_interface_writer.convert( 213 | result) 214 | response.success = True 215 | response.message = '' 216 | else: 217 | response.success = False 218 | response.message = 'No plan found' 219 | 220 | return response 221 | 222 | def pddl_plan_one_shot(self, request, response): 223 | domain_file = '' 224 | problem_file = '' 225 | 226 | if request.plan_request.mode == PDDLPlanRequest.RAW: 227 | domain_file = tempfile.NamedTemporaryFile() 228 | problem_file = tempfile.NamedTemporaryFile() 229 | 230 | with open(domain_file, 'w') as pddl_writer: 231 | pddl_writer.write(request.plan_request.domain) 232 | with open(problem_file, 'w') as pddl_writer: 233 | pddl_writer.write(request.plan_request.problem) 234 | else: 235 | domain_file = request.plan_request.domain 236 | problem_file = request.plan_request.problem 237 | 238 | reader = PDDLReader() 239 | 240 | try: 241 | upf_problem = reader.parse_problem(domain_file, problem_file) 242 | except Exception: 243 | response.success = False 244 | response.message = 'Error parsing problem' 245 | return response 246 | 247 | with OneshotPlanner(problem_kind=upf_problem.kind) as planner: 248 | result = planner.solve(upf_problem) 249 | print('%s returned: %s' % (planner.name, result.plan)) 250 | 251 | if result.plan is not None: 252 | response.plan_result = self._ros2_interface_writer.convert( 253 | result) 254 | response.success = True 255 | response.message = '' 256 | else: 257 | response.success = False 258 | response.message = 'No plan found' 259 | 260 | return response 261 | 262 | def pddl_plan_one_shot_callback(self, goal_handle): 263 | domain_file = '' 264 | problem_file = '' 265 | 266 | if goal_handle.request.plan_request.mode == PDDLPlanRequest.RAW: 267 | domain_file = tempfile.NamedTemporaryFile() 268 | problem_file = tempfile.NamedTemporaryFile() 269 | 270 | with open(domain_file, 'w') as pddl_writer: 271 | pddl_writer.write(goal_handle.request.plan_request.domain) 272 | with open(problem_file, 'w') as pddl_writer: 273 | pddl_writer.write(goal_handle.request.plan_request.problem) 274 | else: 275 | domain_file = goal_handle.request.plan_request.domain 276 | problem_file = goal_handle.request.plan_request.problem 277 | 278 | reader = PDDLReader() 279 | upf_problem = reader.parse_problem(domain_file, problem_file) 280 | 281 | with OneshotPlanner(problem_kind=upf_problem.kind) as planner: 282 | result = planner.solve(upf_problem) 283 | self.get_logger().info( 284 | '%s returned: %s' % 285 | (planner.name, result.plan)) 286 | 287 | feedback_msg = PDDLPlanOneShot.Feedback() 288 | feedback_msg.plan_result = self._ros2_interface_writer.convert( 289 | result) 290 | 291 | goal_handle.publish_feedback(feedback_msg) 292 | goal_handle.succeed() 293 | 294 | result = PDDLPlanOneShot.Result() 295 | result.success = True 296 | result.message = '' 297 | return result 298 | 299 | def plan_one_shot_callback(self, goal_handle): 300 | upf_problem = self._ros2_interface_reader.convert( 301 | goal_handle.request.plan_request.problem) 302 | 303 | with OneshotPlanner(problem_kind=upf_problem.kind) as planner: 304 | result = planner.solve(upf_problem) 305 | self.get_logger().info( 306 | '%s returned: %s' % 307 | (planner.name, result.plan)) 308 | 309 | feedback_msg = PlanOneShot.Feedback() 310 | feedback_msg.plan_result = self._ros2_interface_writer.convert( 311 | result) 312 | 313 | goal_handle.publish_feedback(feedback_msg) 314 | goal_handle.succeed() 315 | 316 | result = PlanOneShot.Result() 317 | result.success = True 318 | result.message = '' 319 | return result 320 | 321 | 322 | def main(args=None): 323 | rclpy.init(args=args) 324 | 325 | upf4ros2_node = UPF4ROS2Node() 326 | 327 | rclpy.spin(upf4ros2_node) 328 | 329 | upf4ros2_node.destroy_node() 330 | rclpy.shutdown() 331 | 332 | 333 | if __name__ == '__main__': 334 | main() 335 | -------------------------------------------------------------------------------- /upf4ros2_demo/README.md: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PlanSys2/UPF4ROS2/77a81582d302da03bdce1f05a95b09a0a5c3cffb/upf4ros2_demo/README.md -------------------------------------------------------------------------------- /upf4ros2_demo/launch/upf4ros2_demo1.launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch_ros.actions import Node 3 | from launch.actions import DeclareLaunchArgument 4 | from launch.substitutions import LaunchConfiguration 5 | from ament_index_python.packages import get_package_share_directory 6 | import yaml 7 | import os 8 | 9 | 10 | def generate_launch_description(): 11 | 12 | pkg_name = "upf4ros2_demo" 13 | 14 | # 15 | # ARGS 16 | # 17 | 18 | points = LaunchConfiguration("wps") 19 | declare_points_cmd = DeclareLaunchArgument( 20 | "wps", 21 | default_value=get_package_share_directory( 22 | pkg_name) + "/params/house.yaml", 23 | description="YAML waypoints file") 24 | 25 | # 26 | # ACTIONS 27 | # 28 | 29 | upf4ros2_navigation_action_cmd = Node( 30 | package=pkg_name, 31 | executable="upf4ros2_navigation_action", 32 | name="upf4ros2_navigation_action", 33 | parameters=[points], 34 | output='screen') 35 | 36 | # 37 | # NODES 38 | # 39 | 40 | upf4ros2_demo_cmd = Node( 41 | package=pkg_name, 42 | executable="upf4ros2_demo1", 43 | name="upf4ros2_demo1", 44 | output='screen') 45 | 46 | ld = LaunchDescription() 47 | ld.add_action(declare_points_cmd) 48 | ld.add_action(upf4ros2_navigation_action_cmd) 49 | ld.add_action(upf4ros2_demo_cmd) 50 | 51 | return ld 52 | -------------------------------------------------------------------------------- /upf4ros2_demo/launch/upf4ros2_demo1_bash.launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch_ros.actions import Node 3 | 4 | 5 | def generate_launch_description(): 6 | 7 | upf4ros2_demo_cmd = Node(package='upf4ros2_demo', 8 | executable='upf4ros2_demo1_bash', 9 | output='screen', 10 | parameters=[ 11 | {'problem_name': 'test'} 12 | ]) 13 | 14 | ld = LaunchDescription() 15 | ld.add_action(upf4ros2_demo_cmd) 16 | 17 | return ld 18 | -------------------------------------------------------------------------------- /upf4ros2_demo/launch/upf4ros2_demo1_pddlfile.launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch_ros.actions import Node 3 | 4 | 5 | def generate_launch_description(): 6 | 7 | upf4ros2_pddl_cmd = Node(package='upf4ros2_demo', 8 | executable='upf4ros2_demo1_pddl', 9 | output='screen', 10 | parameters=[ 11 | {'domain': '/pddl/domain.pddl'}, 12 | {'problem': '/pddl/problem.pddl'} 13 | ]) 14 | 15 | ld = LaunchDescription() 16 | ld.add_action(upf4ros2_pddl_cmd) 17 | 18 | return ld 19 | -------------------------------------------------------------------------------- /upf4ros2_demo/launch/upf4ros2_demo2.launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch_ros.actions import Node 3 | from launch.actions import DeclareLaunchArgument 4 | from launch.substitutions import LaunchConfiguration 5 | from ament_index_python.packages import get_package_share_directory 6 | 7 | 8 | def generate_launch_description(): 9 | 10 | pkg_name = "upf4ros2_demo" 11 | 12 | # 13 | # ARGS 14 | # 15 | 16 | points = LaunchConfiguration("wps") 17 | declare_points_cmd = DeclareLaunchArgument( 18 | "wps", 19 | default_value=get_package_share_directory( 20 | pkg_name) + "/params/house.yaml", 21 | description="YAML waypoints file") 22 | 23 | # 24 | # ACTIONS 25 | # 26 | 27 | upf4ros2_navigation_action_cmd = Node( 28 | package=pkg_name, 29 | executable="upf4ros2_navigation_action", 30 | name="upf4ros2_navigation_action", 31 | parameters=[points], 32 | output='screen') 33 | 34 | # 35 | # NODES 36 | # 37 | 38 | upf4ros2_demo_cmd = Node( 39 | package=pkg_name, 40 | executable="upf4ros2_demo2", 41 | name="upf4ros2_demo2", 42 | output='screen') 43 | 44 | ld = LaunchDescription() 45 | ld.add_action(declare_points_cmd) 46 | ld.add_action(upf4ros2_navigation_action_cmd) 47 | ld.add_action(upf4ros2_demo_cmd) 48 | 49 | return ld 50 | -------------------------------------------------------------------------------- /upf4ros2_demo/launch/upf4ros2_demo3.launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch_ros.actions import Node 3 | from launch.actions import DeclareLaunchArgument 4 | from launch.substitutions import LaunchConfiguration 5 | from ament_index_python.packages import get_package_share_directory 6 | 7 | 8 | def generate_launch_description(): 9 | 10 | pkg_name = "upf4ros2_demo" 11 | 12 | # 13 | # ARGS 14 | # 15 | 16 | points = LaunchConfiguration("wps") 17 | declare_points_cmd = DeclareLaunchArgument( 18 | "wps", 19 | default_value=get_package_share_directory( 20 | pkg_name) + "/params/house.yaml", 21 | description="YAML waypoints file") 22 | 23 | # 24 | # ACTIONS 25 | # 26 | 27 | upf4ros2_navigation_action_cmd = Node( 28 | package=pkg_name, 29 | executable="upf4ros2_navigation_action", 30 | name="upf4ros2_navigation_action", 31 | parameters=[points], 32 | output='screen') 33 | 34 | upf4ros2_check_wp_action_cmd = Node( 35 | package=pkg_name, 36 | executable="upf4ros2_check_wp_action", 37 | name="upf4ros2_check_wp_action", 38 | output='screen') 39 | 40 | # 41 | # NODES 42 | # 43 | 44 | upf4ros2_demo_cmd = Node( 45 | package=pkg_name, 46 | executable="upf4ros2_demo3", 47 | name="upf4ros2_demo3", 48 | output='screen') 49 | 50 | ld = LaunchDescription() 51 | ld.add_action(declare_points_cmd) 52 | ld.add_action(upf4ros2_navigation_action_cmd) 53 | ld.add_action(upf4ros2_check_wp_action_cmd) 54 | ld.add_action(upf4ros2_demo_cmd) 55 | 56 | return ld 57 | -------------------------------------------------------------------------------- /upf4ros2_demo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | upf4ros2_demo 5 | 0.0.0 6 | TODO: Package description 7 | gentlebots 8 | Apache License, Version 2.0 9 | 10 | rclpy 11 | sensor_msgs 12 | geometry_msgs 13 | upf_msgs 14 | 15 | ament_copyright 16 | ament_flake8 17 | ament_pep257 18 | python3-pytest 19 | 20 | 21 | ament_python 22 | 23 | 24 | -------------------------------------------------------------------------------- /upf4ros2_demo/params/house.yaml: -------------------------------------------------------------------------------- 1 | upf4ros2_navigation_action: 2 | ros__parameters: 3 | wps: 4 | - "entrance" 5 | - "5.80" 6 | - "-4.63" 7 | - "-0.77" 8 | - "0.63" 9 | 10 | - "kitchen" 11 | - "7.22" 12 | - "-0.97" 13 | - "0.002" 14 | - "1.0" 15 | 16 | - "livingroom" 17 | - "1.57" 18 | - "0.70" 19 | - "-0.001" 20 | - "1.0" 21 | 22 | - "bedroom" 23 | - "-6.28" 24 | - "-0.14" 25 | - "-0.001" 26 | - "1.0" 27 | 28 | - "room" 29 | - "-7.78" 30 | - "-2.76" 31 | - "-0.001" 32 | - "1.0" 33 | 34 | - "gym" 35 | - "1.85" 36 | - "4.38" 37 | - "-0.005" 38 | - "1.0" -------------------------------------------------------------------------------- /upf4ros2_demo/resource/upf4ros2_demo: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PlanSys2/UPF4ROS2/77a81582d302da03bdce1f05a95b09a0a5c3cffb/upf4ros2_demo/resource/upf4ros2_demo -------------------------------------------------------------------------------- /upf4ros2_demo/resource/upf_problem.sh: -------------------------------------------------------------------------------- 1 | #! /bin/bash 2 | # Script to create a example problem to UPF 3 | 4 | # Create new problem 5 | 6 | ros2 service call /upf4ros2/srv/new_problem upf_msgs/srv/NewProblem "{problem_name: test}" 7 | 8 | # Create the fluent robot_at 9 | 10 | ros2 service call /upf4ros2/srv/add_fluent upf_msgs/srv/AddFluent "{problem_name: test, fluent:{name: robot_at, value_type: up:bool, parameters:[{name: l1, type: Location}]}, default_value:{expressions:[{atom:[{symbol_atom:[], int_atom:[], real_atom:[], boolean_atom:[False]}], type: up:bool, kind: 1}], level:[0]}}" 11 | 12 | # Create the objects l1 and l2 (Location) 13 | 14 | ros2 service call /upf4ros2/srv/add_object upf_msgs/srv/AddObject "{problem_name: test, object:{name: l1, type: Location}}" 15 | 16 | ros2 service call /upf4ros2/srv/add_object upf_msgs/srv/AddObject "{problem_name: test, object:{name: l2, type: Location}}" 17 | 18 | # Set initial values to the fluent robot_at with the objects l1 and l2: robot_at(l1) and robot_at(l2) 19 | # The robot is at l1 but not at l2 20 | 21 | ros2 service call /upf4ros2/srv/set_initial_value upf_msgs/srv/SetInitialValue "{problem_name: test, expression: {expressions:[{atom:[{symbol_atom:['robot_at l1'], int_atom:[], real_atom:[], boolean_atom:[]}], type: up:bool, kind: 5}, {atom:[{symbol_atom:['robot_at'], int_atom:[], real_atom:[], boolean_atom:[]}], type: up:bool, kind: 3}, {atom:[{symbol_atom:['l1'], int_atom:[], real_atom:[], boolean_atom:[]}], type: Location, kind: 1}], level:[0, 1, 1]}, value: {expressions:[{atom:[{symbol_atom:[], int_atom:[], real_atom:[], boolean_atom:[True]}], type: up:bool, kind: 1}], level:[0]}}" 22 | 23 | ros2 service call /upf4ros2/srv/set_initial_value upf_msgs/srv/SetInitialValue "{problem_name: test, expression: {expressions:[{atom:[{symbol_atom:['robot_at l2'], int_atom:[], real_atom:[], boolean_atom:[]}], type: up:bool, kind: 5}, {atom:[{symbol_atom:['robot_at'], int_atom:[], real_atom:[], boolean_atom:[]}], type: up:bool, kind: 3}, {atom:[{symbol_atom:['l2'], int_atom:[], real_atom:[], boolean_atom:[]}], type: Location, kind: 1}], level:[0, 1, 1]}, value: {expressions:[{atom:[{symbol_atom:[], int_atom:[], real_atom:[], boolean_atom:[False]}], type: up:bool, kind: 1}], level:[0]}}" 24 | 25 | # Create the action move to change the location 26 | 27 | ros2 service call /upf4ros2/srv/add_action upf_msgs/srv/AddAction "{problem_name: test, action:{name: move, parameters:[{name: l_from, type: Location},{name: l_to, type: Location}], duration:[], conditions:[{cond: {expressions:[{atom:[], type: up:bool, kind: 5},{atom:[{symbol_atom:['robot_at'], int_atom:[], real_atom:[], boolean_atom:[]}], type: up:bool, kind: 3},{atom:[{symbol_atom:['l_from'], int_atom:[], real_atom:[], boolean_atom:[]}], type: Location, kind: 2}], level:[0, 1, 1]}, span:[]}], effects:[{effect:{kind: 0, fluent: {expressions:[{atom:[], type: up:bool, kind: 5}, {atom:[{symbol_atom:['robot_at'], int_atom:[], real_atom:[], boolean_atom:[]}], type: up:bool, kind: 3}, {atom:[{symbol_atom:['l_from'], int_atom:[], real_atom:[], boolean_atom:[]}], type: Location, kind: 2}], level:[0, 1, 1]}, value: {expressions:[{atom:[{symbol_atom:[], int_atom:[], real_atom:[], boolean_atom:[False]}], type: up:bool, kind: 1}], level:[0]}, condition:{expressions:[{atom:[{symbol_atom:[], int_atom:[], real_atom:[], boolean_atom:[True]}], type: up:bool, kind: 1}], level:[0]}}, occurrence_time:[]}, {effect:{kind: 0, fluent: {expressions:[{atom:[], type: up:bool, kind: 5}, {atom:[{symbol_atom:['robot_at'], int_atom:[], real_atom:[], boolean_atom:[]}], type: up:bool, kind: 3}, {atom:[{symbol_atom:['l_to'], int_atom:[], real_atom:[], boolean_atom:[]}], type: Location, kind: 2}], level:[0, 1, 1]}, value: {expressions:[{atom:[{symbol_atom:[], int_atom:[], real_atom:[], boolean_atom:[True]}], type: up:bool, kind: 1}], level:[0]}, condition:{expressions:[{atom:[{symbol_atom:[], int_atom:[], real_atom:[], boolean_atom:[True]}], type: up:bool, kind: 1}], level:[0]}}, occurrence_time:[]}]}}" 28 | 29 | # Create the goal to the problem 30 | # The robot is at l2 at the end of the problem 31 | 32 | ros2 service call /upf4ros2/add_goal upf_msgs/srv/AddGoal "{problem_name: test, goal:[{goal:{expressions:[{atom:[], type: up:bool, kind: 5}, {atom:[{symbol_atom:['robot_at'], int_atom:[], real_atom:[], boolean_atom:[]}], type: up:bool, kind: 3}, {atom:[{symbol_atom:['l2'], int_atom:[], real_atom:[], boolean_atom:[]}], type: Location, kind: 1}], level:[0, 1, 1]}, timing:[]}], goal_with_cost:[]}" 33 | -------------------------------------------------------------------------------- /upf4ros2_demo/setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script_dir=$base/lib/upf4ros2_demo 3 | [install] 4 | install_scripts=$base/lib/upf4ros2_demo 5 | -------------------------------------------------------------------------------- /upf4ros2_demo/setup.py: -------------------------------------------------------------------------------- 1 | from glob import glob 2 | import os 3 | 4 | from setuptools import setup 5 | 6 | package_name = 'upf4ros2_demo' 7 | 8 | setup( 9 | name=package_name, 10 | version='0.0.0', 11 | packages=[package_name], 12 | data_files=[ 13 | ('share/ament_index/resource_index/packages', 14 | ['resource/' + package_name]), 15 | ('share/' + package_name, ['package.xml']), 16 | (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')), 17 | (os.path.join('share', package_name, 'pddl'), glob('test/pddl/*')), 18 | (os.path.join("share", package_name, "params"), glob("params/*.yaml")), 19 | ], 20 | install_requires=['setuptools'], 21 | zip_safe=True, 22 | maintainer='gentlebots', 23 | maintainer_email='igonzf06@estudiantes.unileon.es', 24 | description='ROS 2 Support for UPF', 25 | license='Apache License, Version 2.0', 26 | tests_require=['pytest'], 27 | entry_points={ 28 | 'console_scripts': [ 29 | 'upf4ros2_demo1 = upf4ros2_demo.upf4ros2_demo1:main', 30 | 'upf4ros2_demo1_pddl = upf4ros2_demo.upf4ros2_demo1_pddl:main', 31 | 'upf4ros2_demo1_bash = upf4ros2_demo.upf4ros2_demo1_bash:main', 32 | 'upf4ros2_demo2 = upf4ros2_demo.upf4ros2_demo2:main', 33 | 'upf4ros2_demo3 = upf4ros2_demo.upf4ros2_demo3:main', 34 | 'upf4ros2_navigation_action = upf4ros2_demo.upf4ros2_navigation_action:main', 35 | 'upf4ros2_check_wp_action = upf4ros2_demo.upf4ros2_check_wp_action:main' 36 | ], 37 | }, 38 | ) 39 | -------------------------------------------------------------------------------- /upf4ros2_demo/test/pddl/domain.pddl: -------------------------------------------------------------------------------- 1 | (define (domain test) 2 | 3 | (:types 4 | location ; service areas, points of interest, navigation goals 5 | ) 6 | 7 | (:predicates 8 | ; the robot is at location ?l 9 | (robot_at ?l - location) 10 | ) 11 | 12 | ; navigation action 13 | (:action move 14 | :parameters (?l_from ?l_to - location) 15 | :precondition (and 16 | (robot_at ?l_from) 17 | ) 18 | :effect (and 19 | (not (robot_at ?l_from)) 20 | (robot_at ?l_to) 21 | ) 22 | ) 23 | 24 | ) -------------------------------------------------------------------------------- /upf4ros2_demo/test/pddl/domain_check_door.pddl: -------------------------------------------------------------------------------- 1 | (define (domain doorbell_attend) 2 | 3 | (:types 4 | wp 5 | sound 6 | door 7 | ) 8 | 9 | (:predicates 10 | ; the robot is at location ?l 11 | (robot_at ?l - wp) 12 | (door_at ?d - door ?l - wp) 13 | (door_checked ?d - door) 14 | (sound_listened ?s - sound) 15 | ) 16 | 17 | ; navigation action 18 | (:durative-action move 19 | :parameters (?l_from ?l_to - location) 20 | :duration (= ?duration 10) 21 | :condition (and 22 | (robot_at ?l_from) 23 | ) 24 | :effect (and 25 | (not (robot_at ?l_from)) 26 | (robot_at ?l_to) 27 | ) 28 | ) 29 | 30 | ; check door action 31 | (:durative-action check_door 32 | :parameters (?d - door ?entrance - wp) 33 | :duration (= ?duration 10) 34 | :condition (and 35 | (door_at ?d ?entrance) 36 | (robot_at ?entrance) 37 | ) 38 | :effect (and 39 | (door_checked ?d) 40 | ) 41 | ) 42 | 43 | ) -------------------------------------------------------------------------------- /upf4ros2_demo/test/pddl/problem.pddl: -------------------------------------------------------------------------------- 1 | (define (problem test_problem) 2 | (:domain test) 3 | (:objects 4 | ; locations 5 | livingroom entrance - location 6 | ) 7 | (:init 8 | ; the robot at start is at livingroom 9 | (robot_at livingroom) 10 | ) 11 | (:goal 12 | (robot_at entrance) 13 | ) 14 | ) -------------------------------------------------------------------------------- /upf4ros2_demo/test/pddl/problem_check_door.pddl: -------------------------------------------------------------------------------- 1 | (define (problem doorbell_attend_problem) 2 | (:domain doorbell_attend) 3 | (:objects 4 | livingroom entrance - wp 5 | door - door 6 | doorbell - sound 7 | ) 8 | (:init 9 | ; the robot at start is at livingroom 10 | (robot_at livingroom) 11 | (door_at door entrance) 12 | ) 13 | (:goal 14 | (door_checked door) 15 | ) 16 | ) -------------------------------------------------------------------------------- /upf4ros2_demo/test/test_copyright.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_copyright.main import main 16 | import pytest 17 | 18 | 19 | # Remove the `skip` decorator once the source file(s) have a copyright header 20 | @pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') 21 | @pytest.mark.copyright 22 | @pytest.mark.linter 23 | def test_copyright(): 24 | rc = main(argv=['.', 'test']) 25 | assert rc == 0, 'Found errors' 26 | -------------------------------------------------------------------------------- /upf4ros2_demo/test/test_flake8.py: -------------------------------------------------------------------------------- 1 | # Copyright 2017 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_flake8.main import main_with_errors 16 | import pytest 17 | 18 | 19 | @pytest.mark.flake8 20 | @pytest.mark.linter 21 | def test_flake8(): 22 | rc, errors = main_with_errors(argv=[]) 23 | assert rc == 0, \ 24 | 'Found %d code style errors / warnings:\n' % len(errors) + \ 25 | '\n'.join(errors) 26 | -------------------------------------------------------------------------------- /upf4ros2_demo/test/test_pep257.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_pep257.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.linter 20 | @pytest.mark.pep257 21 | def test_pep257(): 22 | rc = main(argv=['.', 'test']) 23 | assert rc == 0, 'Found code style errors / warnings' 24 | -------------------------------------------------------------------------------- /upf4ros2_demo/upf4ros2_demo/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PlanSys2/UPF4ROS2/77a81582d302da03bdce1f05a95b09a0a5c3cffb/upf4ros2_demo/upf4ros2_demo/__init__.py -------------------------------------------------------------------------------- /upf4ros2_demo/upf4ros2_demo/upf4ros2_check_wp_action.py: -------------------------------------------------------------------------------- 1 | from typing import List 2 | import rclpy 3 | 4 | from geometry_msgs.msg import Pose 5 | from nav2_msgs.action import NavigateToPose 6 | from upf4ros2_demo_msgs.srv import CallAction 7 | from upf4ros2.ros2_interface_reader import ROS2InterfaceReader 8 | from upf4ros2.ros2_interface_writer import ROS2InterfaceWriter 9 | from unified_planning import model 10 | from unified_planning import shortcuts 11 | from upf_msgs import msg as msgs 12 | from std_msgs.msg import String 13 | from upf4ros2_demo_msgs.srv import CallAction 14 | 15 | from upf_msgs.srv import ( 16 | SetInitialValue, 17 | GetProblem 18 | ) 19 | 20 | from simple_node import Node 21 | 22 | 23 | class CheckWpAction(Node): 24 | 25 | def __init__(self): 26 | super().__init__('upf4ros2_check_wp_action') 27 | 28 | self._problem_name = 'test' 29 | 30 | self._userType = shortcuts.UserType('location') 31 | self._fluent = model.Fluent( 32 | "wp_checked", 33 | shortcuts.BoolType(), 34 | object=self._userType) 35 | self._fluent_robot_at = model.Fluent( 36 | "robot_at", shortcuts.BoolType(), object=self._userType) 37 | 38 | self._ros2_interface_writer = ROS2InterfaceWriter() 39 | self._ros2_interface_reader = ROS2InterfaceReader() 40 | 41 | self._get_problem = self.create_client( 42 | GetProblem, 'upf4ros2/get_problem') 43 | self._set_initial_value = self.create_client( 44 | SetInitialValue, 'upf4ros2/set_initial_value') 45 | 46 | self.create_service( 47 | CallAction, 'check_wp', self.__execute_callback) 48 | 49 | def get_problem(self): 50 | """ get actual state of the problem 51 | Args: 52 | 53 | """ 54 | srv = GetProblem.Request() 55 | srv.problem_name = self._problem_name 56 | 57 | self._get_problem.wait_for_service() 58 | self.res = self._get_problem.call(srv) 59 | # problem = self._ros2_interface_reader.convert(self.future.result().problem) 60 | problem = self.res.problem 61 | return problem 62 | 63 | def set_initial_value(self, fluent, object, value_fluent): 64 | """ set initial value to the fluent 65 | Args: 66 | fluent (up.model.Fluent): fluent 67 | object (up.model.Object): fluent's object 68 | value_fluent (bool): new value 69 | """ 70 | srv = SetInitialValue.Request() 71 | srv.problem_name = self._problem_name 72 | srv.expression = self._ros2_interface_writer.convert(fluent(object)) 73 | 74 | item = msgs.ExpressionItem() 75 | item.atom.append(msgs.Atom()) 76 | item.atom[0].boolean_atom.append(value_fluent) 77 | item.type = 'up:bool' 78 | item.kind = msgs.ExpressionItem.CONSTANT 79 | value = msgs.Expression() 80 | value.expressions.append(item) 81 | value.level.append(0) 82 | 83 | srv.value = value 84 | 85 | self._set_initial_value.wait_for_service() 86 | self.future = self._set_initial_value.call(srv) 87 | 88 | self.get_logger().info( 89 | f'Set {fluent.name}({object.name}) with value: {value_fluent}') 90 | 91 | def __execute_callback(self, request, response): 92 | """ srv callback to call the NavigateToPose action 93 | Args: 94 | request (CallAction.Request): request with the action's name and parameters 95 | response (CallAction.Response): response with the result of the action 96 | Returns: 97 | CallAction.Response: response with the result of the action 98 | """ 99 | problem = self._ros2_interface_reader.convert(self.get_problem()) 100 | 101 | wp = request.parameters[0].symbol_atom[0] 102 | l1 = model.Object(wp, self._userType) 103 | 104 | if str(problem.initial_values[self._fluent_robot_at(l1)]) == 'true': 105 | 106 | self.get_logger().info("Starting action " + request.action_name) 107 | self.get_logger().info("Waypoint " + str(wp) + " checked.") 108 | 109 | self.set_initial_value(self._fluent, l1, True) 110 | response.result = True 111 | else: 112 | self.get_logger().info("Cannot check wp because the wp was not accesible.") 113 | response.result = False 114 | 115 | return response 116 | 117 | 118 | def main(args=None): 119 | rclpy.init(args=args) 120 | 121 | check_wp_action = CheckWpAction() 122 | 123 | check_wp_action.join_spin() 124 | 125 | rclpy.shutdown() 126 | 127 | 128 | if __name__ == '__main__': 129 | main() 130 | -------------------------------------------------------------------------------- /upf4ros2_demo/upf4ros2_demo/upf4ros2_demo1.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from rclpy.action import ActionClient 3 | from rclpy.node import Node 4 | 5 | from unified_planning import model 6 | 7 | from upf4ros2.ros2_interface_reader import ROS2InterfaceReader 8 | from upf4ros2.ros2_interface_writer import ROS2InterfaceWriter 9 | from unified_planning import model 10 | from unified_planning import shortcuts 11 | from upf_msgs import msg as msgs 12 | from upf4ros2_demo_msgs.srv import CallAction 13 | 14 | from upf_msgs.action import ( 15 | PDDLPlanOneShot, 16 | PlanOneShot 17 | ) 18 | 19 | from upf_msgs.srv import ( 20 | AddAction, 21 | AddFluent, 22 | AddGoal, 23 | AddObject, 24 | GetProblem, 25 | NewProblem, 26 | SetInitialValue 27 | ) 28 | 29 | from upf_msgs.srv import PlanOneShot as PlanOneShotSrv 30 | 31 | 32 | class UPF4ROS2Demo1Node(Node): 33 | 34 | def __init__(self): 35 | super().__init__('upf4ros2_demo1') 36 | 37 | self._problem_name = '' 38 | self._plan_result = {} 39 | 40 | self._ros2_interface_writer = ROS2InterfaceWriter() 41 | self._ros2_interface_reader = ROS2InterfaceReader() 42 | 43 | self._plan_one_shot_client = ActionClient( 44 | self, 45 | PlanOneShot, 46 | 'upf4ros2/action/planOneShot') 47 | 48 | self._plan_pddl_one_shot_client = ActionClient( 49 | self, 50 | PDDLPlanOneShot, 51 | 'upf4ros2/action/planOneShotPDDL') 52 | 53 | self._get_problem = self.create_client( 54 | GetProblem, 'upf4ros2/srv/get_problem') 55 | self._new_problem = self.create_client( 56 | NewProblem, 'upf4ros2/srv/new_problem') 57 | self._add_fluent = self.create_client( 58 | AddFluent, 'upf4ros2/srv/add_fluent') 59 | self._add_action = self.create_client( 60 | AddAction, 'upf4ros2/srv/add_action') 61 | self._add_object = self.create_client( 62 | AddObject, 'upf4ros2/srv/add_object') 63 | self._set_initial_value = self.create_client( 64 | SetInitialValue, 'upf4ros2/srv/set_initial_value') 65 | self._add_goal = self.create_client( 66 | AddGoal, 'upf4ros2/add_goal') 67 | self._plan_one_shot_client_srv = self.create_client( 68 | PlanOneShotSrv, 'upf4ros2/srv/planOneShot') 69 | 70 | def new_problem(self, problem_name): 71 | srv = NewProblem.Request() 72 | srv.problem_name = problem_name 73 | 74 | self._new_problem.wait_for_service() 75 | self.future = self._new_problem.call_async(srv) 76 | rclpy.spin_until_future_complete(self, self.future) 77 | 78 | self._problem_name = problem_name 79 | self.get_logger().info( 80 | f'Create the problem with name: {srv.problem_name}') 81 | 82 | def get_problem(self): 83 | srv = GetProblem.Request() 84 | srv.problem_name = self._problem_name 85 | 86 | self._get_problem.wait_for_service() 87 | self.future = self._get_problem.call_async(srv) 88 | rclpy.spin_until_future_complete(self, self.future) 89 | # problem = self._ros2_interface_reader.convert(self.future.result().problem) 90 | problem = self.future.result().problem 91 | return problem 92 | 93 | def add_fluent(self, problem, fluent_name, user_type): 94 | fluent = model.Fluent( 95 | fluent_name, 96 | shortcuts.BoolType(), 97 | object=user_type) 98 | srv = AddFluent.Request() 99 | srv.problem_name = self._problem_name 100 | srv.fluent = self._ros2_interface_writer.convert(fluent, problem) 101 | 102 | item = msgs.ExpressionItem() 103 | item.atom.append(msgs.Atom()) 104 | item.atom[0].boolean_atom.append(False) 105 | item.type = 'up:bool' 106 | item.kind = msgs.ExpressionItem.CONSTANT 107 | value = msgs.Expression() 108 | value.expressions.append(item) 109 | value.level.append(0) 110 | 111 | srv.default_value = value 112 | 113 | self._add_fluent.wait_for_service() 114 | self.future = self._add_fluent.call_async(srv) 115 | rclpy.spin_until_future_complete(self, self.future) 116 | 117 | self.get_logger().info(f'Add fluent: {fluent_name}') 118 | return fluent 119 | 120 | def add_object(self, object_name, user_type): 121 | upf_object = model.Object(object_name, user_type) 122 | srv = AddObject.Request() 123 | srv.problem_name = self._problem_name 124 | srv.object = self._ros2_interface_writer.convert(upf_object) 125 | 126 | self._add_object.wait_for_service() 127 | self.future = self._add_object.call_async(srv) 128 | rclpy.spin_until_future_complete(self, self.future) 129 | 130 | self.get_logger().info(f'Add Object: {object_name}') 131 | 132 | return upf_object 133 | 134 | def set_initial_value(self, fluent, object, value_fluent): 135 | srv = SetInitialValue.Request() 136 | srv.problem_name = self._problem_name 137 | srv.expression = self._ros2_interface_writer.convert(fluent(object)) 138 | 139 | item = msgs.ExpressionItem() 140 | item.atom.append(msgs.Atom()) 141 | item.atom[0].boolean_atom.append(value_fluent) 142 | item.type = 'up:bool' 143 | item.kind = msgs.ExpressionItem.CONSTANT 144 | value = msgs.Expression() 145 | value.expressions.append(item) 146 | value.level.append(0) 147 | 148 | srv.value = value 149 | 150 | self._set_initial_value.wait_for_service() 151 | self.future = self._set_initial_value.call_async(srv) 152 | rclpy.spin_until_future_complete(self, self.future) 153 | 154 | self.get_logger().info( 155 | f'Set {fluent.name}({object.name}) with value: {value_fluent}') 156 | 157 | def add_action(self, action): 158 | srv = AddAction.Request() 159 | srv.problem_name = self._problem_name 160 | srv.action = self._ros2_interface_writer.convert(action) 161 | 162 | self._add_action.wait_for_service() 163 | self.future = self._add_action.call_async(srv) 164 | rclpy.spin_until_future_complete(self, self.future) 165 | 166 | self.get_logger().info(f'Add action: {action.name}') 167 | 168 | def add_goal(self, goal): 169 | srv = AddGoal.Request() 170 | srv.problem_name = self._problem_name 171 | upf_goal = msgs.Goal() 172 | upf_goal.goal = self._ros2_interface_writer.convert(goal) 173 | srv.goal.append(upf_goal) 174 | 175 | self._add_goal.wait_for_service() 176 | self.future = self._add_goal.call_async(srv) 177 | rclpy.spin_until_future_complete(self, self.future) 178 | 179 | self.get_logger().info(f'Set new goal!') 180 | 181 | def get_plan_srv(self): 182 | 183 | problem = self.get_problem() 184 | 185 | self.get_logger().info('Planning...') 186 | srv = PlanOneShotSrv.Request() 187 | srv.problem = problem 188 | 189 | self._plan_one_shot_client_srv.wait_for_service() 190 | 191 | self.future = self._plan_one_shot_client_srv.call_async(srv) 192 | rclpy.spin_until_future_complete(self, self.future) 193 | 194 | plan_result = self.future.result().plan_result 195 | for action in plan_result.plan.actions: 196 | 197 | params = [x.symbol_atom[0] for x in action.parameters] 198 | self.get_logger().info(action.action_name + "(" + ", ".join(params) + ")") 199 | 200 | 201 | def main(args=None): 202 | rclpy.init(args=args) 203 | 204 | # test is the name of the problem for the demo 205 | upf4ros2_demo_node = UPF4ROS2Demo1Node() 206 | 207 | upf4ros2_demo_node.new_problem('test') 208 | problem = upf4ros2_demo_node._ros2_interface_reader.convert( 209 | upf4ros2_demo_node.get_problem()) 210 | 211 | # usertype is the type of the fluent's object 212 | # usertype can be 'up:bool', 'up:integer', 'up:integer[]', 'up:real', 213 | # 'up:real[]', shortcuts.UserType('name') 214 | location = shortcuts.UserType('location') 215 | 216 | robot_at = upf4ros2_demo_node.add_fluent(problem, 'robot_at', location) 217 | 218 | l1 = upf4ros2_demo_node.add_object('livingroom', location) 219 | l2 = upf4ros2_demo_node.add_object('entrance', location) 220 | 221 | upf4ros2_demo_node.set_initial_value(robot_at, l1, True) 222 | upf4ros2_demo_node.set_initial_value(robot_at, l2, False) 223 | 224 | # Define navigation action (InstantaneousAction or DurativeAction) 225 | move = model.InstantaneousAction('move', l_from=location, l_to=location) 226 | # move = model.DurativeAction('move', l_from=location, l_to=location) 227 | # If DurativeAction 228 | # 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) 229 | # move.set_closed_duration_interval(0, 10) 230 | 231 | l_from = move.parameter('l_from') 232 | l_to = move.parameter('l_to') 233 | 234 | move.add_precondition(robot_at(l_from)) 235 | # move.add_condition(model.StartTiming(), robot_at(l_from)) 236 | move.add_effect(robot_at(l_from), False) 237 | move.add_effect(robot_at(l_to), True) 238 | 239 | upf4ros2_demo_node.add_action(move) 240 | 241 | upf4ros2_demo_node.add_goal(robot_at(l2)) 242 | 243 | upf4ros2_demo_node.get_plan_srv() 244 | 245 | problem_updated = upf4ros2_demo_node._ros2_interface_reader.convert( 246 | upf4ros2_demo_node.get_problem()) 247 | upf4ros2_demo_node.get_logger().info(f'{problem_updated}') 248 | 249 | rclpy.spin(upf4ros2_demo_node) 250 | 251 | upf4ros2_demo_node.destroy_node() 252 | rclpy.shutdown() 253 | 254 | 255 | if __name__ == '__main__': 256 | main() 257 | -------------------------------------------------------------------------------- /upf4ros2_demo/upf4ros2_demo/upf4ros2_demo1_bash.py: -------------------------------------------------------------------------------- 1 | import tempfile 2 | from ament_index_python.packages import get_package_share_directory 3 | 4 | import rclpy 5 | from rclpy.action import ActionClient 6 | from rclpy.node import Node 7 | 8 | from upf4ros2.ros2_interface_reader import ROS2InterfaceReader 9 | from upf4ros2.ros2_interface_writer import ROS2InterfaceWriter 10 | from upf_msgs import msg as msgs 11 | 12 | from upf_msgs.action import ( 13 | PlanOneShot 14 | ) 15 | 16 | from upf_msgs.srv import ( 17 | GetProblem, 18 | ) 19 | 20 | from upf_msgs.srv import PlanOneShot as PlanOneShotSrv 21 | 22 | 23 | class UPF4ROS2PlanNode(Node): 24 | 25 | def __init__(self): 26 | super().__init__('upf4ros2_plan') 27 | 28 | self.declare_parameter('problem_name', 'test') 29 | self._problem_name = self.get_parameter('problem_name') 30 | 31 | self._ros2_interface_writer = ROS2InterfaceWriter() 32 | self._ros2_interface_reader = ROS2InterfaceReader() 33 | 34 | self._plan_one_shot_client = ActionClient( 35 | self, 36 | PlanOneShot, 37 | 'upf4ros2/action/planOneShot') 38 | 39 | self._get_problem = self.create_client( 40 | GetProblem, 'upf4ros2/srv/get_problem') 41 | 42 | self._plan_one_shot_client_srv = self.create_client( 43 | PlanOneShotSrv, 'upf4ros2/srv/planOneShot') 44 | 45 | def get_problem(self): 46 | srv = GetProblem.Request() 47 | srv.problem_name = str(self._problem_name.value) 48 | 49 | self._get_problem.wait_for_service() 50 | self.future = self._get_problem.call_async(srv) 51 | rclpy.spin_until_future_complete(self, self.future) 52 | problem = self.future.result().problem 53 | return problem 54 | 55 | def get_plan_action(self): 56 | self.get_logger().info('Planning...') 57 | problem = self.get_problem() 58 | goal_msg = PlanOneShot.Goal() 59 | goal_msg.plan_request.problem = self._ros2_interface_writer.convert( 60 | problem) 61 | 62 | self._plan_one_shot_client.wait_for_server() 63 | self.future = self._plan_one_shot_client.send_goal_async( 64 | goal_msg, feedback_callback=self.feedback_callback) 65 | self.future.add_done_callback(self.goal_response_callback) 66 | 67 | def goal_response_callback(self, future): 68 | goal_handle = future.result() 69 | if not goal_handle.accepted: 70 | self.get_logger().info('Solution not found :(') 71 | return 72 | 73 | self.get_logger().info('Solution found :)') 74 | 75 | def feedback_callback(self, feedback_msg): 76 | feedback = feedback_msg.feedback 77 | 78 | def get_plan_srv(self, problem): 79 | 80 | self.get_logger().info('Planning...') 81 | srv = PlanOneShotSrv.Request() 82 | srv.problem = problem 83 | 84 | self._plan_one_shot_client_srv.wait_for_service() 85 | 86 | self.future = self._plan_one_shot_client_srv.call_async(srv) 87 | rclpy.spin_until_future_complete(self, self.future) 88 | 89 | plan_result = self.future.result().plan_result 90 | for action in plan_result.plan.actions: 91 | 92 | params = [x.symbol_atom[0] for x in action.parameters] 93 | self.get_logger().info(action.action_name + "(" + ", ".join(params) + ")") 94 | 95 | 96 | def main(args=None): 97 | rclpy.init(args=args) 98 | 99 | upf4ros2_plan_node = UPF4ROS2PlanNode() 100 | 101 | upf_problem = upf4ros2_plan_node.get_problem() 102 | problem = upf4ros2_plan_node._ros2_interface_reader.convert(upf_problem) 103 | upf4ros2_plan_node.get_logger().info(f'{problem}') 104 | 105 | upf4ros2_plan_node.get_plan_srv(upf_problem) 106 | rclpy.spin(upf4ros2_plan_node) 107 | 108 | upf4ros2_plan_node.destroy_node() 109 | rclpy.shutdown() 110 | 111 | 112 | if __name__ == '__main__': 113 | main() 114 | -------------------------------------------------------------------------------- /upf4ros2_demo/upf4ros2_demo/upf4ros2_demo1_pddl.py: -------------------------------------------------------------------------------- 1 | import tempfile 2 | import threading 3 | from ament_index_python.packages import get_package_share_directory 4 | 5 | import rclpy 6 | from rclpy.action import ActionClient 7 | from rclpy.node import Node 8 | 9 | from unified_planning import model 10 | from unified_planning.io.pddl_reader import PDDLReader 11 | 12 | from upf4ros2.ros2_interface_reader import ROS2InterfaceReader 13 | from upf4ros2.ros2_interface_writer import ROS2InterfaceWriter 14 | from unified_planning import model 15 | from unified_planning import shortcuts 16 | from upf_msgs import msg as msgs 17 | 18 | from upf_msgs.action import ( 19 | PDDLPlanOneShot, 20 | ) 21 | from upf_msgs.srv import ( 22 | GetProblem, 23 | ) 24 | 25 | from upf_msgs.srv import PDDLPlanOneShot as PDDLPlanOneShotSrv 26 | 27 | 28 | class UPF4ROS2Demo1PDDL(Node): 29 | 30 | def __init__(self): 31 | super().__init__('upf4ros2_demo1_pddl') 32 | 33 | self.declare_parameter('domain', '/pddl/domain.pddl') 34 | self.declare_parameter('problem', '/pddl/problem.pddl') 35 | 36 | self._domain = self.get_parameter('domain') 37 | self._problem = self.get_parameter('problem') 38 | 39 | self._ros2_interface_writer = ROS2InterfaceWriter() 40 | self._ros2_interface_reader = ROS2InterfaceReader() 41 | 42 | self._plan_pddl_one_shot_client = ActionClient( 43 | self, 44 | PDDLPlanOneShot, 45 | 'upf4ros2/planOneShotPDDL') 46 | 47 | self._get_problem = self.create_client( 48 | GetProblem, 'upf4ros2/get_problem') 49 | 50 | self._plan_pddl_one_shot_client_srv = self.create_client( 51 | PDDLPlanOneShotSrv, 'upf4ros2/srv/planOneShotPDDL') 52 | 53 | def get_problem(self): 54 | srv = GetProblem.Request() 55 | srv.problem_name = self._problem_name 56 | 57 | self._get_problem.wait_for_service() 58 | self.future = self._get_problem.call_async(srv) 59 | rclpy.spin_until_future_complete(self, self.future) 60 | problem = self._ros2_interface_reader.convert( 61 | self.future.result().problem) 62 | return problem 63 | 64 | def get_plan_action(self): 65 | 66 | self.get_logger().info('Planning...') 67 | goal_msg = PDDLPlanOneShot.Goal() 68 | goal_msg.plan_request.mode = msgs.PDDLPlanRequest.FILE 69 | goal_msg.plan_request.domain = (get_package_share_directory( 70 | 'upf4ros2_demo') + str(self._domain.value)) 71 | goal_msg.plan_request.problem = (get_package_share_directory( 72 | 'upf4ros2_demo') + str(self._problem.value)) 73 | 74 | self._plan_pddl_one_shot_client.wait_for_server() 75 | 76 | res = self._plan_pddl_one_shot_client.send_goal_async(goal_msg) 77 | res.add_done_callback(self.goal_response_callback) 78 | 79 | return res 80 | 81 | def goal_response_callback(self, future): 82 | goal_handle = future.result() 83 | if not goal_handle.accepted: 84 | self.get_logger().info('Solution not found :(') 85 | return 86 | 87 | self.get_logger().info('Solution found :)') 88 | 89 | def get_plan_srv(self): 90 | 91 | self.get_logger().info('Planning...') 92 | srv = PDDLPlanOneShotSrv.Request() 93 | srv.plan_request.mode = msgs.PDDLPlanRequest.FILE 94 | srv.plan_request.domain = (get_package_share_directory('upf4ros2_demo') 95 | + str(self._domain.value)) 96 | srv.plan_request.problem = (get_package_share_directory( 97 | 'upf4ros2_demo') + str(self._problem.value)) 98 | 99 | self._plan_pddl_one_shot_client_srv.wait_for_service() 100 | 101 | self.future = self._plan_pddl_one_shot_client_srv.call_async(srv) 102 | rclpy.spin_until_future_complete(self, self.future) 103 | 104 | plan_result = self.future.result().plan_result 105 | for action in plan_result.plan.actions: 106 | 107 | params = [x.symbol_atom[0] for x in action.parameters] 108 | self.get_logger().info(action.action_name + "(" + ", ".join(params) + ")") 109 | 110 | 111 | def main(args=None): 112 | rclpy.init(args=args) 113 | 114 | upf4ros2_pddl_node = UPF4ROS2Demo1PDDL() 115 | 116 | upf4ros2_pddl_node.get_plan_srv() 117 | rclpy.spin(upf4ros2_pddl_node) 118 | 119 | upf4ros2_pddl_node.destroy_node() 120 | rclpy.shutdown() 121 | 122 | 123 | if __name__ == '__main__': 124 | main() 125 | -------------------------------------------------------------------------------- /upf4ros2_demo/upf4ros2_demo/upf4ros2_demo2.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from rclpy.action import ActionClient 3 | from rclpy.node import Node 4 | 5 | from unified_planning import model 6 | 7 | from upf4ros2.ros2_interface_reader import ROS2InterfaceReader 8 | from upf4ros2.ros2_interface_writer import ROS2InterfaceWriter 9 | from unified_planning import model 10 | from unified_planning import shortcuts 11 | from upf_msgs import msg as msgs 12 | from upf4ros2_demo_msgs.srv import CallAction 13 | 14 | from upf_msgs.action import ( 15 | PDDLPlanOneShot, 16 | PlanOneShot 17 | ) 18 | 19 | from upf_msgs.srv import ( 20 | AddAction, 21 | AddFluent, 22 | AddGoal, 23 | AddObject, 24 | GetProblem, 25 | NewProblem, 26 | SetInitialValue 27 | ) 28 | 29 | from upf_msgs.srv import PlanOneShot as PlanOneShotSrv 30 | 31 | 32 | class UPF4ROS2DemoNode(Node): 33 | 34 | def __init__(self): 35 | super().__init__('upf4ros2_demo_navigate') 36 | 37 | self._problem_name = '' 38 | self._plan_result = {} 39 | 40 | self._ros2_interface_writer = ROS2InterfaceWriter() 41 | self._ros2_interface_reader = ROS2InterfaceReader() 42 | 43 | self._plan_one_shot_client = ActionClient( 44 | self, 45 | PlanOneShot, 46 | 'upf4ros2/planOneShot') 47 | 48 | self._plan_pddl_one_shot_client = ActionClient( 49 | self, 50 | PDDLPlanOneShot, 51 | 'upf4ros2/planOneShotPDDL') 52 | 53 | self._get_problem = self.create_client( 54 | GetProblem, 'upf4ros2/get_problem') 55 | self._new_problem = self.create_client( 56 | NewProblem, 'upf4ros2/new_problem') 57 | self._add_fluent = self.create_client( 58 | AddFluent, 'upf4ros2/add_fluent') 59 | self._add_action = self.create_client( 60 | AddAction, 'upf4ros2/add_action') 61 | self._add_object = self.create_client( 62 | AddObject, 'upf4ros2/add_object') 63 | self._set_initial_value = self.create_client( 64 | SetInitialValue, 'upf4ros2/set_initial_value') 65 | self._add_goal = self.create_client( 66 | AddGoal, 'upf4ros2/add_goal') 67 | self._plan_one_shot_client_srv = self.create_client( 68 | PlanOneShotSrv, 'upf4ros2/srv/planOneShot') 69 | 70 | def new_problem(self, problem_name): 71 | srv = NewProblem.Request() 72 | srv.problem_name = problem_name 73 | 74 | self._new_problem.wait_for_service() 75 | self.future = self._new_problem.call_async(srv) 76 | rclpy.spin_until_future_complete(self, self.future) 77 | 78 | self._problem_name = problem_name 79 | self.get_logger().info( 80 | f'Create the problem with name: {srv.problem_name}') 81 | 82 | def get_problem(self): 83 | srv = GetProblem.Request() 84 | srv.problem_name = self._problem_name 85 | 86 | self._get_problem.wait_for_service() 87 | self.future = self._get_problem.call_async(srv) 88 | rclpy.spin_until_future_complete(self, self.future) 89 | # problem = self._ros2_interface_reader.convert(self.future.result().problem) 90 | problem = self.future.result().problem 91 | return problem 92 | 93 | def add_fluent(self, problem, fluent_name, user_type): 94 | fluent = model.Fluent( 95 | fluent_name, 96 | shortcuts.BoolType(), 97 | object=user_type) 98 | srv = AddFluent.Request() 99 | srv.problem_name = self._problem_name 100 | srv.fluent = self._ros2_interface_writer.convert(fluent, problem) 101 | 102 | item = msgs.ExpressionItem() 103 | item.atom.append(msgs.Atom()) 104 | item.atom[0].boolean_atom.append(False) 105 | item.type = 'up:bool' 106 | item.kind = msgs.ExpressionItem.CONSTANT 107 | value = msgs.Expression() 108 | value.expressions.append(item) 109 | value.level.append(0) 110 | 111 | srv.default_value = value 112 | 113 | self._add_fluent.wait_for_service() 114 | self.future = self._add_fluent.call_async(srv) 115 | rclpy.spin_until_future_complete(self, self.future) 116 | 117 | self.get_logger().info(f'Add fluent: {fluent_name}') 118 | return fluent 119 | 120 | def add_object(self, object_name, user_type): 121 | upf_object = model.Object(object_name, user_type) 122 | srv = AddObject.Request() 123 | srv.problem_name = self._problem_name 124 | srv.object = self._ros2_interface_writer.convert(upf_object) 125 | 126 | self._add_object.wait_for_service() 127 | self.future = self._add_object.call_async(srv) 128 | rclpy.spin_until_future_complete(self, self.future) 129 | 130 | self.get_logger().info(f'Add Object: {object_name}') 131 | 132 | return upf_object 133 | 134 | def set_initial_value(self, fluent, object, value_fluent): 135 | srv = SetInitialValue.Request() 136 | srv.problem_name = self._problem_name 137 | srv.expression = self._ros2_interface_writer.convert(fluent(object)) 138 | 139 | item = msgs.ExpressionItem() 140 | item.atom.append(msgs.Atom()) 141 | item.atom[0].boolean_atom.append(value_fluent) 142 | item.type = 'up:bool' 143 | item.kind = msgs.ExpressionItem.CONSTANT 144 | value = msgs.Expression() 145 | value.expressions.append(item) 146 | value.level.append(0) 147 | 148 | srv.value = value 149 | 150 | self._set_initial_value.wait_for_service() 151 | self.future = self._set_initial_value.call_async(srv) 152 | rclpy.spin_until_future_complete(self, self.future) 153 | 154 | self.get_logger().info( 155 | f'Set {fluent.name}({object.name}) with value: {value_fluent}') 156 | 157 | def add_action(self, action): 158 | srv = AddAction.Request() 159 | srv.problem_name = self._problem_name 160 | srv.action = self._ros2_interface_writer.convert(action) 161 | 162 | self._add_action.wait_for_service() 163 | self.future = self._add_action.call_async(srv) 164 | rclpy.spin_until_future_complete(self, self.future) 165 | 166 | self.get_logger().info(f'Add action: {action.name}') 167 | 168 | def add_goal(self, goal): 169 | srv = AddGoal.Request() 170 | srv.problem_name = self._problem_name 171 | upf_goal = msgs.Goal() 172 | upf_goal.goal = self._ros2_interface_writer.convert(goal) 173 | srv.goal.append(upf_goal) 174 | 175 | self._add_goal.wait_for_service() 176 | self.future = self._add_goal.call_async(srv) 177 | rclpy.spin_until_future_complete(self, self.future) 178 | 179 | self.get_logger().info(f'Set new goal!') 180 | 181 | def get_plan_action(self): 182 | self.get_logger().info('Planning...') 183 | problem = self.get_problem() 184 | goal_msg = PlanOneShot.Goal() 185 | goal_msg.plan_request.problem = self._ros2_interface_writer.convert( 186 | problem) 187 | 188 | self._plan_one_shot_client.wait_for_server() 189 | self.future = self._plan_one_shot_client.send_goal_async( 190 | goal_msg, feedback_callback=self.feedback_callback) 191 | self.future.add_done_callback(self.goal_response_callback) 192 | 193 | def goal_response_callback(self, future): 194 | goal_handle = future.result() 195 | if not goal_handle.accepted: 196 | self.get_logger().info('Solution not found :(') 197 | return 198 | 199 | self.get_logger().info('Solution found :)') 200 | 201 | def feedback_callback(self, feedback_msg): 202 | feedback = feedback_msg.feedback 203 | 204 | for action in feedback.plan_result.plan.actions: 205 | 206 | params = [x.symbol_atom[0] for x in action.parameters] 207 | self.get_logger().info( 208 | action.action_name + "(" + params[0] + ", " + params[1] + ")") 209 | 210 | client = self.create_client(CallAction, action.action_name) 211 | while not client.wait_for_service(timeout_sec=1.0): 212 | self.get_logger().info('service not available, waiting again...') 213 | 214 | req = CallAction.Request() 215 | req.action_name = action.action_name 216 | req.parameters = action.parameters 217 | 218 | self.res = client.call_async(req) 219 | rclpy.spin_until_future_complete(self, self.res) 220 | 221 | def get_plan_srv(self): 222 | 223 | problem = self.get_problem() 224 | 225 | self.get_logger().info('Planning...') 226 | srv = PlanOneShotSrv.Request() 227 | srv.problem = problem 228 | 229 | self._plan_one_shot_client_srv.wait_for_service() 230 | 231 | self.future = self._plan_one_shot_client_srv.call_async(srv) 232 | rclpy.spin_until_future_complete(self, self.future) 233 | 234 | plan_result = self.future.result().plan_result 235 | for action in plan_result.plan.actions: 236 | 237 | params = [x.symbol_atom[0] for x in action.parameters] 238 | self.get_logger().info(action.action_name + "(" + ", ".join(params) + ")") 239 | client = self.create_client(CallAction, action.action_name) 240 | while not client.wait_for_service(timeout_sec=1.0): 241 | self.get_logger().info('service not available, waiting again...') 242 | 243 | req = CallAction.Request() 244 | req.action_name = action.action_name 245 | req.parameters = action.parameters 246 | 247 | self.res = client.call_async(req) 248 | rclpy.spin_until_future_complete(self, self.res) 249 | 250 | if self.res.result().result: 251 | self.get_logger().info('Action completed') 252 | else: 253 | self.get_logger().info('Action cancelled!') 254 | 255 | 256 | def main(args=None): 257 | rclpy.init(args=args) 258 | 259 | # test is the name of the problem for the demo 260 | upf4ros2_demo_node = UPF4ROS2DemoNode() 261 | 262 | upf4ros2_demo_node.new_problem('test') 263 | problem = upf4ros2_demo_node._ros2_interface_reader.convert( 264 | upf4ros2_demo_node.get_problem()) 265 | 266 | # usertype is the type of the fluent's object 267 | # usertype can be 'up:bool', 'up:integer', 'up:integer[]', 'up:real', 268 | # 'up:real[]', shortcuts.UserType('name') 269 | location = shortcuts.UserType('location') 270 | 271 | robot_at = upf4ros2_demo_node.add_fluent(problem, 'robot_at', location) 272 | 273 | l1 = upf4ros2_demo_node.add_object('livingroom', location) 274 | l2 = upf4ros2_demo_node.add_object('entrance', location) 275 | 276 | upf4ros2_demo_node.set_initial_value(robot_at, l1, True) 277 | upf4ros2_demo_node.set_initial_value(robot_at, l2, False) 278 | 279 | # Define navigation action (InstantaneousAction or DurativeAction) 280 | move = model.InstantaneousAction('move', l_from=location, l_to=location) 281 | # move = model.DurativeAction('move', l_from=location, l_to=location) 282 | # If DurativeAction 283 | # 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) 284 | # move.set_closed_duration_interval(0, 10) 285 | 286 | l_from = move.parameter('l_from') 287 | l_to = move.parameter('l_to') 288 | 289 | move.add_precondition(robot_at(l_from)) 290 | # move.add_condition(model.StartTiming(), robot_at(l_from)) 291 | move.add_effect(robot_at(l_from), False) 292 | move.add_effect(robot_at(l_to), True) 293 | 294 | upf4ros2_demo_node.add_action(move) 295 | 296 | upf4ros2_demo_node.add_goal(robot_at(l2)) 297 | 298 | upf4ros2_demo_node.get_plan_srv() 299 | 300 | problem_updated = upf4ros2_demo_node._ros2_interface_reader.convert( 301 | upf4ros2_demo_node.get_problem()) 302 | upf4ros2_demo_node.get_logger().info(f'{problem_updated}') 303 | 304 | rclpy.spin(upf4ros2_demo_node) 305 | 306 | upf4ros2_demo_node.destroy_node() 307 | rclpy.shutdown() 308 | 309 | 310 | if __name__ == '__main__': 311 | main() 312 | -------------------------------------------------------------------------------- /upf4ros2_demo/upf4ros2_demo/upf4ros2_demo3.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from rclpy.action import ActionClient 3 | # from rclpy.node import Node 4 | from simple_node import Node 5 | 6 | from unified_planning import model 7 | 8 | from upf4ros2.ros2_interface_reader import ROS2InterfaceReader 9 | from upf4ros2.ros2_interface_writer import ROS2InterfaceWriter 10 | from unified_planning import model 11 | from unified_planning import shortcuts 12 | from upf_msgs import msg as msgs 13 | from upf4ros2_demo_msgs.srv import CallAction 14 | 15 | from upf_msgs.srv import ( 16 | AddAction, 17 | AddFluent, 18 | AddGoal, 19 | AddObject, 20 | GetProblem, 21 | NewProblem, 22 | SetInitialValue 23 | ) 24 | 25 | from upf_msgs.srv import PlanOneShot as PlanOneShotSrv 26 | # varios puntos (lanzar el plan con todos los puntos// ir uno a uno ) 27 | # checkear puntos, para eso tiene que estar en el punto y luego checkearlo 28 | 29 | 30 | class UPF4ROS2DemoNode(Node): 31 | 32 | def __init__(self): 33 | super().__init__('upf4ros2_demo_navigate') 34 | 35 | self._problem_name = '' 36 | self._plan_result = {} 37 | 38 | self._ros2_interface_writer = ROS2InterfaceWriter() 39 | self._ros2_interface_reader = ROS2InterfaceReader() 40 | 41 | self._get_problem = self.create_client( 42 | GetProblem, 'upf4ros2/get_problem') 43 | self._new_problem = self.create_client( 44 | NewProblem, 'upf4ros2/new_problem') 45 | self._add_fluent = self.create_client( 46 | AddFluent, 'upf4ros2/add_fluent') 47 | self._add_action = self.create_client( 48 | AddAction, 'upf4ros2/add_action') 49 | self._add_object = self.create_client( 50 | AddObject, 'upf4ros2/add_object') 51 | self._set_initial_value = self.create_client( 52 | SetInitialValue, 'upf4ros2/set_initial_value') 53 | self._add_goal = self.create_client( 54 | AddGoal, 'upf4ros2/add_goal') 55 | self._plan_one_shot_client_srv = self.create_client( 56 | PlanOneShotSrv, 'upf4ros2/srv/planOneShot') 57 | 58 | def new_problem(self, problem_name): 59 | srv = NewProblem.Request() 60 | srv.problem_name = problem_name 61 | 62 | self._new_problem.wait_for_service() 63 | self.future = self._new_problem.call(srv) 64 | 65 | self._problem_name = problem_name 66 | self.get_logger().info( 67 | f'Create the problem with name: {srv.problem_name}') 68 | 69 | def get_problem(self): 70 | srv = GetProblem.Request() 71 | srv.problem_name = self._problem_name 72 | 73 | self._get_problem.wait_for_service() 74 | self.res = self._get_problem.call(srv) 75 | # problem = self._ros2_interface_reader.convert(self.future.result().problem) 76 | problem = self.res.problem 77 | return problem 78 | 79 | def add_fluent(self, problem, fluent_name, user_type): 80 | fluent = model.Fluent( 81 | fluent_name, 82 | shortcuts.BoolType(), 83 | object=user_type) 84 | srv = AddFluent.Request() 85 | srv.problem_name = self._problem_name 86 | srv.fluent = self._ros2_interface_writer.convert(fluent, problem) 87 | 88 | item = msgs.ExpressionItem() 89 | item.atom.append(msgs.Atom()) 90 | item.atom[0].boolean_atom.append(False) 91 | item.type = 'up:bool' 92 | item.kind = msgs.ExpressionItem.CONSTANT 93 | value = msgs.Expression() 94 | value.expressions.append(item) 95 | value.level.append(0) 96 | 97 | srv.default_value = value 98 | 99 | self._add_fluent.wait_for_service() 100 | self.future = self._add_fluent.call(srv) 101 | 102 | self.get_logger().info(f'Add fluent: {fluent_name}') 103 | return fluent 104 | 105 | def add_object(self, object_name, user_type): 106 | upf_object = model.Object(object_name, user_type) 107 | srv = AddObject.Request() 108 | srv.problem_name = self._problem_name 109 | srv.object = self._ros2_interface_writer.convert(upf_object) 110 | 111 | self._add_object.wait_for_service() 112 | self.future = self._add_object.call(srv) 113 | 114 | self.get_logger().info(f'Add Object: {object_name}') 115 | 116 | return upf_object 117 | 118 | def set_initial_value(self, fluent, object, value_fluent): 119 | srv = SetInitialValue.Request() 120 | srv.problem_name = self._problem_name 121 | srv.expression = self._ros2_interface_writer.convert(fluent(object)) 122 | 123 | item = msgs.ExpressionItem() 124 | item.atom.append(msgs.Atom()) 125 | item.atom[0].boolean_atom.append(value_fluent) 126 | item.type = 'up:bool' 127 | item.kind = msgs.ExpressionItem.CONSTANT 128 | value = msgs.Expression() 129 | value.expressions.append(item) 130 | value.level.append(0) 131 | 132 | srv.value = value 133 | 134 | self._set_initial_value.wait_for_service() 135 | self.future = self._set_initial_value.call(srv) 136 | 137 | self.get_logger().info( 138 | f'Set {fluent.name}({object.name}) with value: {value_fluent}') 139 | 140 | def add_action(self, action): 141 | srv = AddAction.Request() 142 | srv.problem_name = self._problem_name 143 | srv.action = self._ros2_interface_writer.convert(action) 144 | 145 | self._add_action.wait_for_service() 146 | self.future = self._add_action.call(srv) 147 | 148 | self.get_logger().info(f'Add action: {action.name}') 149 | 150 | def add_goal(self, goal): 151 | srv = AddGoal.Request() 152 | srv.problem_name = self._problem_name 153 | upf_goal = msgs.Goal() 154 | upf_goal.goal = self._ros2_interface_writer.convert(goal) 155 | srv.goal.append(upf_goal) 156 | 157 | self._add_goal.wait_for_service() 158 | self.future = self._add_goal.call(srv) 159 | 160 | self.get_logger().info(f'Set new goal!') 161 | 162 | def __cancel_callback(self, action): 163 | # return to old wp 164 | if action.action_name == 'move': 165 | client = self.create_client(CallAction, action.action_name) 166 | while not client.wait_for_service(timeout_sec=1.0): 167 | self.get_logger().info('service not available, waiting again...') 168 | 169 | req = CallAction.Request() 170 | req.action_name = action.action_name 171 | req.parameters = action.parameters[::-1] 172 | self.res = client.call(req) 173 | if self.res.result: 174 | self.get_logger().info('Return to origin wp') 175 | else: 176 | self.get_logger().info('Can not return to the origin wp!') 177 | 178 | def get_plan_srv(self): 179 | 180 | problem = self.get_problem() 181 | 182 | self.get_logger().info('Planning...') 183 | srv = PlanOneShotSrv.Request() 184 | srv.problem = problem 185 | 186 | self._plan_one_shot_client_srv.wait_for_service() 187 | 188 | self.res = self._plan_one_shot_client_srv.call(srv) 189 | 190 | plan_result = self.res.plan_result 191 | 192 | for action in plan_result.plan.actions: 193 | 194 | params = [x.symbol_atom[0] for x in action.parameters] 195 | self.get_logger().info(action.action_name + "(" + ", ".join(params) + ")") 196 | 197 | for action in plan_result.plan.actions: 198 | 199 | client = self.create_client(CallAction, action.action_name) 200 | while not client.wait_for_service(timeout_sec=1.0): 201 | self.get_logger().info('service not available, waiting again...') 202 | req = CallAction.Request() 203 | req.action_name = action.action_name 204 | req.parameters = action.parameters 205 | 206 | self.res = client.call(req) 207 | 208 | if self.res.result: 209 | self.get_logger().info('Action completed') 210 | else: 211 | self.get_logger().info('Action cancelled!') 212 | self.__cancel_callback(action) 213 | 214 | 215 | def main(args=None): 216 | rclpy.init(args=args) 217 | 218 | # test is the name of the problem for the demo 219 | upf4ros2_demo_node = UPF4ROS2DemoNode() 220 | 221 | upf4ros2_demo_node.new_problem('test') 222 | problem = upf4ros2_demo_node._ros2_interface_reader.convert( 223 | upf4ros2_demo_node.get_problem()) 224 | 225 | # usertype is the type of the fluent's object 226 | # usertype can be 'up:bool', 'up:integer', 'up:integer[]', 'up:real', 227 | # 'up:real[]', shortcuts.UserType('name') 228 | location = shortcuts.UserType('location') 229 | 230 | robot_at = upf4ros2_demo_node.add_fluent(problem, 'robot_at', location) 231 | wp_checked = upf4ros2_demo_node.add_fluent(problem, 'wp_checked', location) 232 | 233 | livingroom = upf4ros2_demo_node.add_object('livingroom', location) 234 | entrance = upf4ros2_demo_node.add_object('entrance', location) 235 | kitchen = upf4ros2_demo_node.add_object('kitchen', location) 236 | bedroom = upf4ros2_demo_node.add_object('bedroom', location) 237 | room = upf4ros2_demo_node.add_object('room', location) 238 | gym = upf4ros2_demo_node.add_object('gym', location) 239 | 240 | upf4ros2_demo_node.set_initial_value(robot_at, livingroom, True) 241 | upf4ros2_demo_node.set_initial_value(robot_at, entrance, False) 242 | upf4ros2_demo_node.set_initial_value(wp_checked, entrance, False) 243 | 244 | # Define navigation action (InstantaneousAction or DurativeAction) 245 | move = model.InstantaneousAction('move', l_from=location, l_to=location) 246 | # move = model.DurativeAction('move', l_from=location, l_to=location) 247 | # If DurativeAction 248 | # 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) 249 | # move.set_closed_duration_interval(0, 10) 250 | 251 | l_from = move.parameter('l_from') 252 | l_to = move.parameter('l_to') 253 | 254 | move.add_precondition(robot_at(l_from)) 255 | # move.add_condition(model.StartTiming(), robot_at(l_from)) 256 | move.add_effect(robot_at(l_from), False) 257 | move.add_effect(robot_at(l_to), True) 258 | 259 | # ------------------------------------- # 260 | # Define check wp action 261 | check_wp = model.InstantaneousAction('check_wp', wp=location) 262 | wp = check_wp.parameter('wp') 263 | check_wp.add_precondition(robot_at(wp)) 264 | check_wp.add_effect(wp_checked(wp), True) 265 | 266 | upf4ros2_demo_node.add_action(move) 267 | upf4ros2_demo_node.add_action(check_wp) 268 | 269 | # upf4ros2_demo_node.add_goal(robot_at(l2)) 270 | upf4ros2_demo_node.add_goal(wp_checked(entrance)) 271 | upf4ros2_demo_node.add_goal(wp_checked(gym)) 272 | upf4ros2_demo_node.add_goal(wp_checked(kitchen)) 273 | 274 | problem_old_upf = upf4ros2_demo_node.get_problem() 275 | problem_old = upf4ros2_demo_node._ros2_interface_reader.convert( 276 | problem_old_upf) 277 | upf4ros2_demo_node.get_logger().info(f'{problem_old}') 278 | 279 | upf4ros2_demo_node.get_plan_srv() 280 | 281 | problem_updated = upf4ros2_demo_node._ros2_interface_reader.convert( 282 | upf4ros2_demo_node.get_problem()) 283 | upf4ros2_demo_node.get_logger().info(f'{problem_updated}') 284 | 285 | upf4ros2_demo_node.join_spin() 286 | 287 | rclpy.shutdown() 288 | 289 | 290 | if __name__ == '__main__': 291 | main() 292 | -------------------------------------------------------------------------------- /upf4ros2_demo/upf4ros2_demo/upf4ros2_navigation_action.py: -------------------------------------------------------------------------------- 1 | from typing import List 2 | import rclpy 3 | 4 | from geometry_msgs.msg import Pose 5 | from nav2_msgs.action import NavigateToPose 6 | from upf4ros2_demo_msgs.srv import CallAction 7 | from upf4ros2.ros2_interface_reader import ROS2InterfaceReader 8 | from upf4ros2.ros2_interface_writer import ROS2InterfaceWriter 9 | from unified_planning import model 10 | from unified_planning import shortcuts 11 | from upf_msgs import msg as msgs 12 | from upf4ros2_demo_msgs.srv import CallAction 13 | 14 | from upf_msgs.srv import ( 15 | SetInitialValue, 16 | ) 17 | 18 | from simple_node import Node 19 | 20 | 21 | class NavigationAction(Node): 22 | 23 | def __init__(self): 24 | super().__init__('upf4ros2_navigation_action') 25 | 26 | self.__wp_dict = {} 27 | 28 | wps_param_name = "wps" 29 | 30 | # declaring params 31 | self.declare_parameter(wps_param_name, ['']) 32 | 33 | # getting params 34 | waypoints = self.get_parameter( 35 | wps_param_name).get_parameter_value().string_array_value 36 | 37 | # load points 38 | self.load_wps(waypoints) 39 | 40 | self._problem_name = 'test' 41 | 42 | self._userType = shortcuts.UserType('location') 43 | self._fluent = model.Fluent( 44 | "robot_at", 45 | shortcuts.BoolType(), 46 | object=self._userType) 47 | 48 | self._ros2_interface_writer = ROS2InterfaceWriter() 49 | self._ros2_interface_reader = ROS2InterfaceReader() 50 | 51 | self.__nav_to_pose_client = self.create_action_client( 52 | NavigateToPose, 53 | '/navigate_to_pose') 54 | 55 | self._set_initial_value = self.create_client( 56 | SetInitialValue, 'upf4ros2/set_initial_value') 57 | 58 | self.create_service( 59 | CallAction, 'move', self.__execute_callback) 60 | 61 | def load_wps(self, waypoints: List[str]): 62 | """ load waypoints of list strings into a dictionary of floats 63 | Args: 64 | points (List[str]): list of points 65 | """ 66 | 67 | if not waypoints: 68 | return 69 | 70 | for i in range(0, len(waypoints), 5): 71 | self.__wp_dict[waypoints[i]] = Pose() 72 | self.__wp_dict[waypoints[i]].position.x = float(waypoints[i + 1]) 73 | self.__wp_dict[waypoints[i]].position.y = float(waypoints[i + 2]) 74 | self.__wp_dict[waypoints[i]].orientation.z = float( 75 | waypoints[i + 3]) 76 | self.__wp_dict[waypoints[i]].orientation.w = float( 77 | waypoints[i + 4]) 78 | 79 | def set_initial_value(self, fluent, object, value_fluent): 80 | """ set initial value to the fluent 81 | Args: 82 | fluent (up.model.Fluent): fluent 83 | object (up.model.Object): fluent's object 84 | value_fluent (bool): new value 85 | """ 86 | srv = SetInitialValue.Request() 87 | srv.problem_name = self._problem_name 88 | srv.expression = self._ros2_interface_writer.convert(fluent(object)) 89 | 90 | item = msgs.ExpressionItem() 91 | item.atom.append(msgs.Atom()) 92 | item.atom[0].boolean_atom.append(value_fluent) 93 | item.type = 'up:bool' 94 | item.kind = msgs.ExpressionItem.CONSTANT 95 | value = msgs.Expression() 96 | value.expressions.append(item) 97 | value.level.append(0) 98 | 99 | srv.value = value 100 | 101 | self._set_initial_value.wait_for_service() 102 | self.future = self._set_initial_value.call(srv) 103 | 104 | self.get_logger().info( 105 | f'Set {fluent.name}({object.name}) with value: {value_fluent}') 106 | 107 | def __execute_callback(self, request, response): 108 | """ srv callback to call the NavigateToPose action 109 | Args: 110 | request (CallAction.Request): request with the action's name and parameters 111 | response (CallAction.Response): response with the result of the action 112 | Returns: 113 | CallAction.Response: response with the result of the action 114 | """ 115 | l1 = model.Object(request.parameters[0].symbol_atom[0], self._userType) 116 | l2 = model.Object(request.parameters[1].symbol_atom[0], self._userType) 117 | 118 | self.get_logger().info("Starting action " + request.action_name) 119 | 120 | self.get_logger().info("Waiting for 'NavigateToPose' action server") 121 | while not self.__nav_to_pose_client.wait_for_server(): 122 | self.get_logger().info("'NavigateToPose' action server not available, waiting...") 123 | 124 | wp = request.parameters[1].symbol_atom[0] 125 | goal_msg = NavigateToPose.Goal() 126 | goal_msg.pose.pose = self.__wp_dict[wp] 127 | goal_msg.pose.header.frame_id = "map" 128 | 129 | self.get_logger().info('Navigating to goal: ' + str(wp)) 130 | 131 | self.__nav_to_pose_client.send_goal(goal_msg) 132 | self.__nav_to_pose_client.wait_for_result() 133 | 134 | if self.__nav_to_pose_client.is_succeeded(): 135 | self.get_logger().info("Goal to " + str(wp) + " done") 136 | self.set_initial_value(self._fluent, l1, False) 137 | self.set_initial_value(self._fluent, l2, True) 138 | response.result = True 139 | else: 140 | self.get_logger().info("Goal to " + str(wp) + " was rejected!") 141 | response.result = False 142 | 143 | return response 144 | 145 | 146 | def main(args=None): 147 | rclpy.init(args=args) 148 | 149 | navigation_action = NavigationAction() 150 | 151 | navigation_action.join_spin() 152 | 153 | rclpy.shutdown() 154 | 155 | 156 | if __name__ == '__main__': 157 | main() 158 | -------------------------------------------------------------------------------- /upf4ros2_demo_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(upf4ros2_demo_msgs) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | find_package(rosidl_default_generators REQUIRED) 11 | find_package(upf_msgs REQUIRED) 12 | # uncomment the following section in order to fill in 13 | # further dependencies manually. 14 | # find_package( REQUIRED) 15 | 16 | rosidl_generate_interfaces(${PROJECT_NAME} 17 | "srv/CallAction.srv" 18 | DEPENDENCIES upf_msgs 19 | ) 20 | 21 | if(BUILD_TESTING) 22 | find_package(ament_lint_auto REQUIRED) 23 | # the following line skips the linter which checks for copyrights 24 | # comment the line when a copyright and license is added to all source files 25 | set(ament_cmake_copyright_FOUND TRUE) 26 | # the following line skips cpplint (only works in a git repo) 27 | # comment the line when this package is in a git repo and when 28 | # a copyright and license is added to all source files 29 | set(ament_cmake_cpplint_FOUND TRUE) 30 | ament_lint_auto_find_test_dependencies() 31 | endif() 32 | 33 | ament_package() 34 | -------------------------------------------------------------------------------- /upf4ros2_demo_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | upf4ros2_demo_msgs 5 | 0.0.0 6 | TODO: Package description 7 | gentlebots 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | upf_msgs 12 | ament_lint_auto 13 | ament_lint_common 14 | rosidl_default_generators 15 | 16 | rosidl_default_runtime 17 | 18 | rosidl_interface_packages 19 | 20 | ament_cmake 21 | 22 | 23 | -------------------------------------------------------------------------------- /upf4ros2_demo_msgs/srv/CallAction.srv: -------------------------------------------------------------------------------- 1 | string action_name 2 | upf_msgs/Atom[] parameters 3 | --- 4 | bool result -------------------------------------------------------------------------------- /upf_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(upf_msgs) 2 | 3 | cmake_minimum_required(VERSION 3.5) 4 | 5 | find_package(ament_cmake REQUIRED) 6 | find_package(builtin_interfaces REQUIRED) 7 | find_package(rosidl_default_generators REQUIRED) 8 | 9 | rosidl_generate_interfaces(${PROJECT_NAME} 10 | "msg/AbstractTaskDeclaration.msg" 11 | "msg/Effect.msg" 12 | "msg/Variable.msg" 13 | "msg/Task.msg" 14 | "msg/PlanGenerationResult.msg" 15 | "msg/Method.msg" 16 | "msg/ObjectDeclaration.msg" 17 | "msg/ActionInstance.msg" 18 | "msg/Interval.msg" 19 | "msg/TimedEffect.msg" 20 | "msg/Duration.msg" 21 | "msg/Real.msg" 22 | "msg/PDDLPlanRequest.msg" 23 | "msg/TypeDeclaration.msg" 24 | "msg/Action.msg" 25 | "msg/PlanRequest.msg" 26 | "msg/PlanRequestRemote.msg" 27 | "msg/Hierarchy.msg" 28 | "msg/LogMessage.msg" 29 | "msg/ExpressionItem.msg" 30 | "msg/ValidationResult.msg" 31 | "msg/CompilerResult.msg" 32 | "msg/Condition.msg" 33 | "msg/Expression.msg" 34 | "msg/GoalWithCost.msg" 35 | "msg/Plan.msg" 36 | "msg/Assignment.msg" 37 | "msg/Fluent.msg" 38 | "msg/Problem.msg" 39 | "msg/Goal.msg" 40 | "msg/Atom.msg" 41 | "msg/Schedule.msg" 42 | "msg/Timepoint.msg" 43 | "msg/EffectExpression.msg" 44 | "msg/ValidationRequest.msg" 45 | "msg/TaskNetwork.msg" 46 | "msg/TimeInterval.msg" 47 | "msg/Timing.msg" 48 | "msg/Parameter.msg" 49 | "msg/Metric.msg" 50 | "msg/ValidationMetric.msg" 51 | "srv/GetProblem.srv" 52 | "srv/NewProblem.srv" 53 | "srv/SetProblem.srv" 54 | "srv/AddFluent.srv" 55 | "srv/AddAction.srv" 56 | "srv/AddObject.srv" 57 | "srv/SetInitialValue.srv" 58 | "srv/AddGoal.srv" 59 | "srv/PDDLPlanOneShot.srv" 60 | "srv/PlanOneShot.srv" 61 | "action/PDDLPlanOneShot.action" 62 | "action/PlanOneShot.action" 63 | "action/PlanOneShotRemote.action" 64 | DEPENDENCIES builtin_interfaces 65 | ) 66 | 67 | ament_export_dependencies(rosidl_default_runtime) 68 | ament_package() 69 | -------------------------------------------------------------------------------- /upf_msgs/action/PDDLPlanOneShot.action: -------------------------------------------------------------------------------- 1 | ## A plan request to the engine. 2 | ## The engine replies with a stream of N `Answer` messages where: 3 | ## - the first (N-1) message are of type `IntermediateReport` 4 | ## - the last message is of type `FinalReport` 5 | 6 | upf_msgs/PDDLPlanRequest plan_request 7 | --- 8 | bool success 9 | string message 10 | --- 11 | upf_msgs/PlanGenerationResult plan_result 12 | -------------------------------------------------------------------------------- /upf_msgs/action/PlanOneShot.action: -------------------------------------------------------------------------------- 1 | ## A plan request to the engine. 2 | ## The engine replies with a stream of N `Answer` messages where: 3 | ## - the first (N-1) message are of type `IntermediateReport` 4 | ## - the last message is of type `FinalReport` 5 | 6 | upf_msgs/PlanRequest plan_request 7 | --- 8 | bool success 9 | string message 10 | --- 11 | upf_msgs/PlanGenerationResult plan_result 12 | -------------------------------------------------------------------------------- /upf_msgs/action/PlanOneShotRemote.action: -------------------------------------------------------------------------------- 1 | ## A plan request to the engine. 2 | ## The engine replies with a stream of N `Answer` messages where: 3 | ## - the first (N-1) message are of type `IntermediateReport` 4 | ## - the last message is of type `FinalReport` 5 | 6 | upf_msgs/PlanRequestRemote plan_request 7 | --- 8 | bool success 9 | string message 10 | --- 11 | upf_msgs/PlanGenerationResult plan_result 12 | -------------------------------------------------------------------------------- /upf_msgs/msg/AbstractTaskDeclaration.msg: -------------------------------------------------------------------------------- 1 | ## Declares an abstract task together with its expected parameters. 2 | ## 3 | ## Example: goto(robot: Robot, destination: Location) 4 | 5 | # Example: "goto" 6 | string name 7 | 8 | # Example: 9 | # - robot: Robot 10 | # - destination: Location 11 | upf_msgs/Parameter[] parameters 12 | -------------------------------------------------------------------------------- /upf_msgs/msg/Action.msg: -------------------------------------------------------------------------------- 1 | ## Unified action representation that represents any kind of actions. 2 | 3 | # Action name. E.g. "move" 4 | string name 5 | 6 | # Typed and named parameters of the action. 7 | upf_msgs/Parameter[] parameters 8 | 9 | # If set, the action is durative. Otherwise it is instantaneous. 10 | # features: DURATIVE_ACTIONS 11 | upf_msgs/Duration[<=1] duration 12 | 13 | # Conjunction of conditions that must hold for the action to be applicable. 14 | upf_msgs/Condition[] conditions 15 | 16 | # Conjunction of effects as a result of applying this action. 17 | upf_msgs/Effect[] effects 18 | -------------------------------------------------------------------------------- /upf_msgs/msg/ActionInstance.msg: -------------------------------------------------------------------------------- 1 | ## Representation of an action instance that appears in a plan. 2 | 3 | # Optional. A unique identifier of the action that might be used to refer to it (e.g. in HTN plans). 4 | string id 5 | # name of the action 6 | string action_name 7 | # Parameters of the action instance, required to be constants. 8 | upf_msgs/Atom[] parameters 9 | 10 | bool time_triggered 11 | 12 | # Start time of the action. The default 0 value is OK in the case of non-temporal planning 13 | # feature: [DURATIVE_ACTIONS] 14 | upf_msgs/Real start_time 15 | # End time of the action. The default 0 value is OK in the case of non-temporal planning 16 | # feature: [DURATIVE_ACTIONS] 17 | upf_msgs/Real end_time 18 | -------------------------------------------------------------------------------- /upf_msgs/msg/Assignment.msg: -------------------------------------------------------------------------------- 1 | ## An assignment of a value to a fluent, as it appears in the initial state definition. 2 | 3 | # State variable that is assigned the `value`. 4 | # It should be an expression of the STATE_VARIABLE kind for which all parameters are of the CONSTANT kind. 5 | upf_msgs/Expression fluent 6 | # An expression of the CONSTANT kind, denoting the value take by the state variable. 7 | upf_msgs/Expression value 8 | -------------------------------------------------------------------------------- /upf_msgs/msg/Atom.msg: -------------------------------------------------------------------------------- 1 | string[<=1] symbol_atom 2 | int64[<=1] int_atom 3 | upf_msgs/Real[<=1] real_atom 4 | bool[<=1] boolean_atom 5 | -------------------------------------------------------------------------------- /upf_msgs/msg/CompilerResult.msg: -------------------------------------------------------------------------------- 1 | ## Message sent by the grounder. 2 | 3 | # The problem generated by the Compiler 4 | upf_msgs/Problem problem 5 | # The map_back_plan field is a map from the ActionInstance of the 6 | # compiled problem to the original ActionInstance. 7 | string[] map_back_plan_keys 8 | upf_msgs/ActionInstance[] map_back_plan_values 9 | 10 | # Optional. Information given by the engine to the user. 11 | upf_msgs/LogMessage[] log_messages 12 | 13 | # Synthetic description of the engine that generated this message. 14 | string engine 15 | -------------------------------------------------------------------------------- /upf_msgs/msg/Condition.msg: -------------------------------------------------------------------------------- 1 | upf_msgs/Expression cond 2 | 3 | # Optional. Must be set for durative actions where it specifies the temporal interval 4 | # over which when the condition should hold. 5 | # features: DURATIVE_ACTIONS 6 | upf_msgs/TimeInterval[<=1] span 7 | -------------------------------------------------------------------------------- /upf_msgs/msg/Duration.msg: -------------------------------------------------------------------------------- 1 | # The duration of the action can be freely chosen within the indicated bounds 2 | upf_msgs/Interval controllable_in_bounds 3 | -------------------------------------------------------------------------------- /upf_msgs/msg/Effect.msg: -------------------------------------------------------------------------------- 1 | ## Representation of an effect that allows qualifying the effect expression, e.g., to make it a conditional effect. 2 | 3 | # Required. The actual effect that should take place. 4 | upf_msgs/EffectExpression effect 5 | 6 | # Optional. If the effect is within a durative action, the following must be set and will specify when the effect takes place. 7 | # features: DURATIVE_ACTIONS 8 | upf_msgs/Timing[<=1] occurrence_time 9 | -------------------------------------------------------------------------------- /upf_msgs/msg/EffectExpression.msg: -------------------------------------------------------------------------------- 1 | ## An effect expression is of the form `FLUENT OP VALUE`. 2 | ## We explicitly restrict the different types of effects by setting the allowed operators. 3 | 4 | # The `fluent` is set to the corresponding `value` 5 | uint8 ASSIGN=0 6 | # The `fluent` is increased by the amount `value` 7 | # features: INCREASE_EFFECTS 8 | uint8 INCREASE=1 9 | # The `fluent` is decreased by the amount `value` 10 | # features: DECREASE_EFFECTS 11 | uint8 DECREASE=2 12 | 13 | uint8 kind 14 | 15 | # Expression that must be of the STATE_VARIABLE kind. 16 | upf_msgs/Expression fluent 17 | upf_msgs/Expression value 18 | upf_msgs/Variable[] forall 19 | 20 | # Optional. If the effect is conditional, then the following field must be set. 21 | # In this case, the `effect` will only be applied if the `condition`` holds. 22 | # If the effect is unconditional, the effect is set to the constant 'true' value. 23 | # features: CONDITIONAL_EFFECT 24 | upf_msgs/Expression condition 25 | -------------------------------------------------------------------------------- /upf_msgs/msg/Expression.msg: -------------------------------------------------------------------------------- 1 | upf_msgs/ExpressionItem[] expressions 2 | uint8[] level 3 | -------------------------------------------------------------------------------- /upf_msgs/msg/ExpressionItem.msg: -------------------------------------------------------------------------------- 1 | ## The kind of an expression, which gives information related to its structure. 2 | 3 | # If non-empty, the expression is a single atom. 4 | # For instance `3`, `+`, `kitchen`, `at-robot`, ... 5 | upf_msgs/Atom[<=1] atom 6 | # If the `atom` field is empty, then the expression is a list of sub-expressions, 7 | # typically representing the application of some arguments to a function or fluent. 8 | # For instance `(+ 1 3)`, (at-robot l1)`, `(>= (battery_level) 20)` 9 | 10 | # Type of the expression. For instance "int", "location", ... 11 | string type 12 | 13 | # Kind of the expression, specifying the content of the expression. 14 | # This is intended to facilitate parsing of the expression. 15 | 16 | # Default value, should not be used. Drop it if we are sure to never need it. 17 | uint8 UNKNOWN=0 18 | 19 | # Constant atom. For instance `3` or `kitchen` (where `kitchen` is an object defined in the problem) 20 | uint8 CONSTANT=1 21 | 22 | # Atom symbol representing a parameter from an outer scope. For instance `from` that would appear inside a `(move from to - location)` action. 23 | uint8 PARAMETER=2 24 | 25 | # Atom symbol representing a variable from an outer scope. 26 | # This is typically used to represent the variables that are existentially or universally qualified in expressions. 27 | uint8 VARIABLE=7 28 | 29 | # Atom symbol representing a fluent of the problem. For instance `at-robot`. 30 | uint8 FLUENT_SYMBOL=3 31 | 32 | # Atom representing a function. For instance `+`, `=`, `and`, ... 33 | uint8 FUNCTION_SYMBOL=4 34 | 35 | # List. Application of some parameters to a fluent symbol. For instance `(at-robot l1)` or `(battery-charged)` 36 | # The first element of the list must be a FLUENT_SYMBOL 37 | uint8 STATE_VARIABLE=5 38 | 39 | # List. The expression is the application of some parameters to a function. For instance `(+ 1 3)`. 40 | # The first element of the list must be a FUNCTION_SYMBOL 41 | uint8 FUNCTION_APPLICATION=6 42 | 43 | # Atom symbol. Unique identifier of a task or action in the current scope. 44 | uint8 CONTAINER_ID=8 45 | 46 | uint8 kind 47 | -------------------------------------------------------------------------------- /upf_msgs/msg/Fluent.msg: -------------------------------------------------------------------------------- 1 | ## A state-dependent variable. 2 | 3 | string name 4 | # Return type of the fluent. 5 | string value_type "up:bool" 6 | # Typed and named parameters of the fluent. 7 | upf_msgs/Parameter[] parameters 8 | 9 | # If non-empty, then any state variable using this fluent that is not explicitly given a value in the initial state 10 | # will be assumed to have this default value. 11 | # This allows mimicking the closed world assumption by setting a "false" default value to predicates. 12 | # Note that in the initial state of the problem message, it is assumed that all default values are set. 13 | upf_msgs/Expression[<=1] default_value 14 | -------------------------------------------------------------------------------- /upf_msgs/msg/Goal.msg: -------------------------------------------------------------------------------- 1 | ## A Goal is currently an expression that must hold either: 2 | ## - in the final state, 3 | ## - over a specific temporal interval (under the `timed_goals` features) 4 | 5 | # Goal expression that must hold in the final state. 6 | upf_msgs/Expression goal 7 | 8 | # Optional. If specified the goal should hold over the specified temporal interval (instead of on the final state). 9 | # features: TIMED_GOALS 10 | upf_msgs/TimeInterval[<=1] timing 11 | -------------------------------------------------------------------------------- /upf_msgs/msg/GoalWithCost.msg: -------------------------------------------------------------------------------- 1 | ## Represents a goal associated with a cost, used to define oversubscription planning. 2 | 3 | # Goal expression 4 | upf_msgs/Expression goal 5 | # The cost 6 | upf_msgs/Real cost 7 | -------------------------------------------------------------------------------- /upf_msgs/msg/Hierarchy.msg: -------------------------------------------------------------------------------- 1 | ## Represents the hierarchical part of a problem. 2 | ## features: hierarchical 3 | 4 | upf_msgs/AbstractTaskDeclaration[] abstract_tasks 5 | upf_msgs/Method[] methods 6 | upf_msgs/TaskNetwork initial_task_network 7 | -------------------------------------------------------------------------------- /upf_msgs/msg/Interval.msg: -------------------------------------------------------------------------------- 1 | ## A contiguous slice of time represented as an interval `[lower, upper]` where `lower` and `upper` are time references. 2 | ## The `is_left_open` and `is_right_open` fields indicate whether the interval is 3 | ## opened on left and right side respectively. 4 | 5 | bool is_left_open 6 | upf_msgs/Expression lower 7 | bool is_right_open 8 | upf_msgs/Expression upper 9 | -------------------------------------------------------------------------------- /upf_msgs/msg/LogMessage.msg: -------------------------------------------------------------------------------- 1 | ## A freely formatted logging message. 2 | ## Each message is annotated with its criticality level from the minimal (DEBUG) to the maximal (ERROR). 3 | ## Criticality level is expected to be used by an end user to decide the level of verbosity. 4 | 5 | uint8 DEBUG=0 6 | uint8 INFO=1 7 | uint8 WARNING=2 8 | uint8 ERROR=3 9 | 10 | uint8 level 11 | string message -------------------------------------------------------------------------------- /upf_msgs/msg/Method.msg: -------------------------------------------------------------------------------- 1 | # A method describes one possible way of achieving a task. 2 | # 3 | # Example: A method that make a "move" action and recursively calls itself until reaching the destination. 4 | 5 | # A name that uniquely identify the method. 6 | # This is mostly used for user facing output or plan validation. 7 | # 8 | # Example: "m-recursive-goto" 9 | string name 10 | 11 | # Example: [robot: Robot, source: Location, intermediate: Location, destination: Location] 12 | Parameter[] parameters 13 | 14 | # The task that is achieved by the method. 15 | # A subset of the parameters of the method will typically be used to 16 | # define the task that is achieved. 17 | # 18 | # Example: goto(robot, destination) 19 | upf_msgs/Task achieved_task 20 | 21 | # A set of subtasks that should be achieved to carry out the method. 22 | # Note that the order of subtasks is irrelevant and that any ordering constraint should be 23 | # specified in the `constraints` field. 24 | # 25 | # Example: 26 | # - t1: (move robot source intermediate) 27 | # - t2: goto(robot destination) 28 | upf_msgs/Task[] subtasks 29 | 30 | # Constraints enable the definition of ordering constraints as well as constraints 31 | # on the allowed instantiation of the method's parameters. 32 | # 33 | # Example: 34 | # - end(t1) < start(t2) 35 | # - source != intermediate 36 | upf_msgs/Expression[] constraints 37 | 38 | # Conjunction of conditions that must hold for the method to be applicable. 39 | # As for the conditions of actions, these can be temporally qualified to refer to intermediate timepoints. 40 | # In addition to the start/end of the method, the temporal qualification might refer to the start/end of 41 | # one of the subtasks using its identifier. 42 | # 43 | # Example: 44 | # - [start] loc(robot) == source 45 | # - [end(t1)] loc(robot) == intermediate 46 | # - [end] loc(robot) == destination 47 | upf_msgs/Condition[] conditions 48 | -------------------------------------------------------------------------------- /upf_msgs/msg/Metric.msg: -------------------------------------------------------------------------------- 1 | 2 | # Minimize the action costs expressed in the `action_costs` field 3 | uint8 MINIMIZE_ACTION_COSTS=0 4 | 5 | # Minimize the length of the resulting sequential plan 6 | uint8 MINIMIZE_SEQUENTIAL_PLAN_LENGTH=1 7 | 8 | # Minimize the makespan in case of temporal planning 9 | # features: durative_actions 10 | uint8 MINIMIZE_MAKESPAN=2 11 | 12 | # Minimize the value of the expression defined in the `expression` field 13 | uint8 MINIMIZE_EXPRESSION_ON_FINAL_STATE=3 14 | 15 | # Maximize the value of the expression defined in the `expression` field 16 | uint8 MAXIMIZE_EXPRESSION_ON_FINAL_STATE=4 17 | 18 | # Maximize the number of goals reached, weighted by cost 19 | uint8 OVERSUBSCRIPTION=5 20 | 21 | uint8 kind 22 | 23 | 24 | # Expression to minimize/maximize in the final state. 25 | # Empty, if the `kind` is not {MIN/MAX}IMIZE_EXPRESSION_ON_FINAL_STATE 26 | Expression expression 27 | 28 | # If `kind == MINIMIZE_ACTION_COSTS``, then each action is associated to a cost expression. 29 | # 30 | # TODO: Document what is allowed in the expression. See issue #134 31 | # In particular, for this metric to be useful in many practical problems, the cost expression 32 | # should allow referring to the action parameters (and possibly the current state at the action start/end). 33 | # This is very awkward to do in this setting where the expression is detached from its scope. 34 | upf_msgs/Action[] action_cost_names 35 | upf_msgs/Expression[] action_cost_expr 36 | 37 | upf_msgs/Expression[<=1] default_action_cost 38 | 39 | # List of goals used to define the oversubscription planning problem. 40 | # Empty, if the `kind` is not OVERSUBSCRIPTION 41 | upf_msgs/GoalWithCost[] goals -------------------------------------------------------------------------------- /upf_msgs/msg/ObjectDeclaration.msg: -------------------------------------------------------------------------------- 1 | ## Declares an object with the given name and type. 2 | 3 | # Name of the object. 4 | string name 5 | # Type of the object. 6 | # The type must have been previously declared in the problem definition. 7 | string type 8 | -------------------------------------------------------------------------------- /upf_msgs/msg/PDDLPlanRequest.msg: -------------------------------------------------------------------------------- 1 | # Problem that should be solved. 2 | 3 | # Specify if the domain/model string contains a file path, or the content 4 | uint8 FILE=0 5 | uint8 RAW=1 6 | uint8 mode 7 | 8 | string domain 9 | string problem 10 | 11 | uint8 SATISFIABLE=0 12 | uint8 SOLVED_OPTIMALLY=1 13 | uint8 resolution_mode 14 | 15 | # Max allowed runtime time in seconds. 16 | float64 timeout 17 | 18 | # Engine specific options to be passed to the engine 19 | string engine_option_names 20 | string engine_option_values 21 | -------------------------------------------------------------------------------- /upf_msgs/msg/Parameter.msg: -------------------------------------------------------------------------------- 1 | ## Parameter of a fluent or of an action 2 | 3 | # Name of the parameter. 4 | string name 5 | # Type of the parameter. 6 | string type 7 | -------------------------------------------------------------------------------- /upf_msgs/msg/Plan.msg: -------------------------------------------------------------------------------- 1 | # An ordered sequence of actions that appear in the plan. 2 | # The order of the actions in the list must be compatible with the partial order of the start times. 3 | # In case of non-temporal planning, this allows having all start time at 0 and only rely on the order in this sequence. 4 | uint8 kind 5 | upf_msgs/ActionInstance[] actions 6 | -------------------------------------------------------------------------------- /upf_msgs/msg/PlanGenerationResult.msg: -------------------------------------------------------------------------------- 1 | ## Message sent by engine. 2 | ## Contains the engine exit status as well as the best plan found if any. 3 | 4 | 5 | # ==== Engine stopped normally ====== 6 | 7 | # Valid plan found 8 | # The `plan` field must be set. 9 | uint8 SOLVED_SATISFICING=0 10 | # Plan found with optimality guarantee 11 | # The `plan` field must be set and contains an optimal solution. 12 | uint8 SOLVED_OPTIMALLY=1 13 | # No plan exists 14 | uint8 UNSOLVABLE_PROVEN=2 15 | # The engine was not able to find a solution but does not give any guarantee that none exist 16 | # (i.e. the engine might not be complete) 17 | uint8 UNSOLVABLE_INCOMPLETELY=3 18 | 19 | # ====== Engine exited before making any conclusion ==== 20 | # Search stopped before concluding SOLVED_OPTIMALLY or UNSOLVABLE_PROVEN 21 | # If a plan was found, it might be reported in the `plan` field 22 | 23 | # The engine ran out of time 24 | uint8 TIMEOUT=13 25 | # The engine ran out of memory 26 | uint8 MEMOUT=14 27 | # The engine faced an internal error. 28 | uint8 INTERNAL_ERROR=15 29 | # The problem submitted is not supported by the engine. 30 | uint8 UNSUPPORTED_PROBLEM=16 31 | 32 | # ====== Intermediate answer ====== 33 | # This Answer is an Intermediate Answer and not a Final one 34 | uint8 INTERMEDIATE=17 35 | 36 | uint8 status 37 | 38 | # Optional. Best plan found if any. 39 | upf_msgs/Plan plan 40 | 41 | # A set of engine specific values that can be reported, for instance 42 | # - "grounding-time": "10ms" 43 | # - "expanded-states": "1290" 44 | string[] metric_names 45 | string[] metric_values 46 | 47 | # Optional log messages about the engine's activity. 48 | # Note that it should not be expected that logging messages are visible to the end user. 49 | # If used in conjunction with INTERNAL_ERROR or UNSUPPORTED_PROBLEM, it would be expected to have at least one log message at the ERROR level. 50 | upf_msgs/LogMessage[] log_messages 51 | 52 | # Synthetic description of the engine that generated this message. 53 | string engine_name 54 | -------------------------------------------------------------------------------- /upf_msgs/msg/PlanRequest.msg: -------------------------------------------------------------------------------- 1 | upf_msgs/Problem problem 2 | 3 | uint8 SATISFIABLE=0 4 | uint8 SOLVED_OPTIMALLY=1 5 | uint8 resolution_mode 6 | 7 | # Max allowed runtime time in seconds. 8 | float64 timeout 9 | 10 | # Engine specific options to be passed to the engine 11 | string engine_option_names 12 | string engine_option_values 13 | -------------------------------------------------------------------------------- /upf_msgs/msg/PlanRequestRemote.msg: -------------------------------------------------------------------------------- 1 | string problem 2 | 3 | uint8 SATISFIABLE=0 4 | uint8 SOLVED_OPTIMALLY=1 5 | uint8 resolution_mode 6 | 7 | # Max allowed runtime time in seconds. 8 | float64 timeout 9 | 10 | # Engine specific options to be passed to the engine 11 | string engine_option_names 12 | string engine_option_values 13 | -------------------------------------------------------------------------------- /upf_msgs/msg/Problem.msg: -------------------------------------------------------------------------------- 1 | string domain_name 2 | string problem_name 3 | upf_msgs/TypeDeclaration[] types 4 | upf_msgs/Fluent[] fluents 5 | upf_msgs/ObjectDeclaration[] objects 6 | 7 | # List of actions in the domain. 8 | upf_msgs/Action[] actions 9 | 10 | # Initial state, including default values of state variables. 11 | upf_msgs/Assignment[] initial_state 12 | 13 | # Facts and effects that are expected to occur strictly later than the initial state. 14 | # features: TIMED_EFFECT 15 | upf_msgs/TimedEffect[] timed_effects 16 | 17 | # Goals of the planning problem. 18 | upf_msgs/Goal[] goals 19 | 20 | # The plan quality metrics 21 | upf_msgs/Metric[] metrics 22 | 23 | 24 | # If the problem is hierarchical, defines the tasks and methods as well as the initial task network. 25 | # features: hierarchical 26 | upf_msgs/Hierarchy[<=1] hierarchy 27 | 28 | # all features of the problem 29 | uint8[] features 30 | 31 | upf_msgs/Expression[] trajectory_constraints 32 | 33 | ## Features of the problem. 34 | ## Features are essential in that not supporting a feature `X` should allow disregarding any field tagged with `features: [X]`. 35 | 36 | # PROBLEM_CLASS 37 | uint8 ACTION_BASED=0 38 | uint8 HIERARCHICAL=26 39 | uint8 SCHEDULING = 56 40 | # PROBLEM_TYPE 41 | uint8 SIMPLE_NUMERIC_PLANNING=30 42 | uint8 GENERAL_NUMERIC_PLANNING=31 43 | # TIME 44 | uint8 CONTINUOUS_TIME=1 45 | uint8 DISCRETE_TIME=2 46 | uint8 INTERMEDIATE_CONDITIONS_AND_EFFECTS=3 47 | uint8 EXTERNAL_CONDITIONS_AND_EFFECTS = 39 48 | uint8 TIMED_EFFECTS=4 49 | uint8 TIMED_GOALS=5 50 | uint8 DURATION_INEQUALITIES=6 51 | uint8 SELF_OVERLAPPING = 47 52 | # EXPRESSION_DURATION 53 | uint8 STATIC_FLUENTS_IN_DURATIONS=27 54 | uint8 FLUENTS_IN_DURATIONS=28 55 | uint8 REAL_TYPE_DURATIONS = 62 56 | uint8 INT_TYPE_DURATIONS = 63 57 | # NUMBERS 58 | uint8 CONTINUOUS_NUMBERS=7 59 | uint8 DISCRETE_NUMBERS=8 60 | uint8 BOUNDED_TYPES = 38 61 | # CONDITIONS_KIND 62 | uint8 NEGATIVE_CONDITIONS=9 63 | uint8 DISJUNCTIVE_CONDITIONS=10 64 | uint8 EQUALITIES=11 65 | uint8 EXISTENTIAL_CONDITIONS=12 66 | uint8 UNIVERSAL_CONDITIONS=13 67 | # EFFECTS_KIND 68 | uint8 CONDITIONAL_EFFECTS=14 69 | uint8 INCREASE_EFFECTS=15 70 | uint8 DECREASE_EFFECTS=16 71 | uint8 STATIC_FLUENTS_IN_BOOLEAN_ASSIGNMENTS = 41 72 | uint8 STATIC_FLUENTS_IN_NUMERIC_ASSIGNMENTS = 42 73 | uint8 STATIC_FLUENTS_IN_OBJECT_ASSIGNMENTS = 57 74 | uint8 FLUENTS_IN_BOOLEAN_ASSIGNMENTS = 43 75 | uint8 FLUENTS_IN_NUMERIC_ASSIGNMENTS = 44 76 | uint8 FLUENTS_IN_OBJECT_ASSIGNMENTS = 58 77 | uint8 FORALL_EFFECTS = 59 78 | # TYPING 79 | uint8 FLAT_TYPING=17 80 | uint8 HIERARCHICAL_TYPING=18 81 | # FLUENTS_TYPE 82 | uint8 NUMERIC_FLUENTS=19 83 | uint8 OBJECT_FLUENTS=20 84 | uint8 INT_FLUENTS=60 85 | uint8 REAL_FLUENTS=61 86 | # PARAMETERS 87 | uint8 BOOL_FLUENT_PARAMETERS = 50 88 | uint8 BOUNDED_INT_FLUENT_PARAMETERS = 51 89 | uint8 BOOL_ACTION_PARAMETERS = 52 90 | uint8 BOUNDED_INT_ACTION_PARAMETERS = 53 91 | uint8 UNBOUNDED_INT_ACTION_PARAMETERS = 54 92 | uint8 REAL_ACTION_PARAMETERS = 55 93 | # QUALITY_METRICS 94 | uint8 ACTIONS_COST=21 95 | uint8 FINAL_VALUE=22 96 | uint8 MAKESPAN=23 97 | uint8 PLAN_LENGTH=24 98 | uint8 OVERSUBSCRIPTION=29 99 | uint8 TEMPORAL_OVERSUBSCRIPTION = 40 100 | # ACTION_COST_KIND 101 | uint8 STATIC_FLUENTS_IN_ACTIONS_COST = 45 102 | uint8 FLUENTS_IN_ACTIONS_COST = 46 103 | uint8 REAL_NUMBERS_IN_ACTIONS_COST = 64 104 | uint8 INT_NUMBERS_IN_ACTIONS_COST = 65 105 | # OVERSUBSCRIPTION_KIND 106 | uint8 REAL_NUMBERS_IN_OVERSUBSCRIPTION = 66 107 | uint8 INT_NUMBERS_IN_OVERSUBSCRIPTION = 67 108 | # SIMULATED_ENTITIES 109 | uint8 SIMULATED_EFFECTS=25 110 | # CONSTRAINTS_KIND 111 | uint8 TRAJECTORY_CONSTRAINTS = 48 112 | uint8 STATE_INVARIANTS = 49 113 | # HIERARCHICAL 114 | uint8 METHOD_PRECONDITIONS = 32 115 | uint8 TASK_NETWORK_CONSTRAINTS = 33 116 | uint8 INITIAL_TASK_NETWORK_VARIABLES = 34 117 | uint8 TASK_ORDER_TOTAL = 35 118 | uint8 TASK_ORDER_PARTIAL = 36 119 | uint8 TASK_ORDER_TEMPORAL = 37 120 | # INITIAL_STATE 121 | uint8 UNDEFINED_INITIAL_NUMERIC = 68 122 | uint8 UNDEFINED_INITIAL_SYMBOLIC = 69 123 | -------------------------------------------------------------------------------- /upf_msgs/msg/Real.msg: -------------------------------------------------------------------------------- 1 | ## Representation of a constant real number, as the fraction `(numerator / denominator)`. 2 | ## A real should be in its canonical form (with smallest possible denominator). 3 | ## Notably, if this number is an integer, then it is guaranteed that `denominator == 1`. 4 | 5 | int64 numerator 6 | int64 denominator 1 7 | -------------------------------------------------------------------------------- /upf_msgs/msg/Schedule.msg: -------------------------------------------------------------------------------- 1 | string[] activities_name 2 | upf_msgs/Expression[] variable_assignments -------------------------------------------------------------------------------- /upf_msgs/msg/Task.msg: -------------------------------------------------------------------------------- 1 | ## Representation of an abstract or primitive task that should be achieved, 2 | ## required either in the initial task network or as a subtask of a method. 3 | ## 4 | ## Example: task of sending a `robot` to the KITCHEN 5 | ## - t1: goto(robot, KITCHEN) 6 | 7 | # Identifier of the task, required to be unique in the method/task-network where the task appears. 8 | # The `id` is notably used to refer to the start/end of the task. 9 | # 10 | # Example: t1 11 | string id 12 | 13 | # Name of the task that should be achieved. It might either 14 | # - an abstract task if the name is the one of a task declared in the problem 15 | # - a primitive task if the name is the one of an action declared in the problem 16 | # 17 | # Example: 18 | # - "goto" (abstract task) 19 | # - "move" (action / primitive task) 20 | string task_name 21 | 22 | # Example: (for a "goto" task) 23 | # - robot (a parameter from an outer scope) 24 | # - KITCHEN (a constant symbol in the problem) 25 | upf_msgs/Expression[] parameters 26 | -------------------------------------------------------------------------------- /upf_msgs/msg/TaskNetwork.msg: -------------------------------------------------------------------------------- 1 | ## A task network defines a set of subtasks and associated constraints. 2 | ## It is intended to be used to define the initial task network of the hierarchical problem. 3 | ## 4 | ## Example: an arbitrary robot should go to the KITCHEN before time 100 5 | 6 | # robot: Location 7 | upf_msgs/Parameter[] variables 8 | # t1: goto(robot, KITCHEN) 9 | upf_msgs/Task[] subtasks 10 | # end(t1) <= 100 11 | upf_msgs/Expression[] constraints 12 | -------------------------------------------------------------------------------- /upf_msgs/msg/TimeInterval.msg: -------------------------------------------------------------------------------- 1 | ## A contiguous slice of time represented as an interval `[lower, upper]` where `lower` and `upper` are time references. 2 | ## The `is_left_open` and `is_right_open` fields indicate whether the interval is 3 | ## opened on left and right side respectively. 4 | 5 | bool is_left_open 6 | upf_msgs/Timing lower 7 | bool is_right_open 8 | upf_msgs/Timing upper 9 | -------------------------------------------------------------------------------- /upf_msgs/msg/TimedEffect.msg: -------------------------------------------------------------------------------- 1 | ## Represents an effect that will occur sometime beyond the initial state. (similar to timed initial literals) 2 | 3 | # Required. An effect expression that will take place sometime in the future (i.e. not at the intial state) as specified by the temporal qualifiation. 4 | upf_msgs/EffectExpression[] effect 5 | # Required. Temporal qualification denoting when the timed fact will occur. 6 | upf_msgs/Timing occurrence_time 7 | -------------------------------------------------------------------------------- /upf_msgs/msg/Timepoint.msg: -------------------------------------------------------------------------------- 1 | ## Symbolic reference to an absolute time. 2 | ## It might represent: 3 | ## - the time of the initial/final state, or 4 | ## - the start/end of the containing action. 5 | ## 6 | ## It is currently composed of a single field whose interpretation might be context dependent 7 | ## (e.g. "START" refers to the start of the containing action). 8 | ## 9 | ## In the future, it could be extended to refer, e.g., to the start of a particular action/subtask 10 | ## by adding an additional field with the identifier of an action/subtask. 11 | 12 | # Global start of the planning problem. This is context independent and represents the time at which the initial state holds. 13 | uint8 GLOBAL_START=0 14 | # Global end of the planning problem. This is context independent and represents the time at which the final state holds. 15 | uint8 GLOBAL_END=1 16 | # Start of the container (typically the action or method) in which this symbol occurs 17 | uint8 START=2 18 | # End of the container (typically the action or method) in which this symbol occurs 19 | uint8 END=3 20 | 21 | uint8 kind 22 | 23 | # If non-empty, identifies the container of which we are extracting the start/end timepoint. 24 | # In the context of a task-network or of a method, this could be the `id` of one of the subtasks. 25 | # feature: hierarchies 26 | string container_id 27 | -------------------------------------------------------------------------------- /upf_msgs/msg/Timing.msg: -------------------------------------------------------------------------------- 1 | ## Represents a time (`timepoint` + `delay`), that is a time defined relatively to a particular `timepoint`. 2 | ## Note that an absolute time can be defined by setting the `delay` relative to the `GLOBAL_START`` which is the reference time. 3 | 4 | upf_msgs/Timepoint timepoint 5 | upf_msgs/Real[<=1] delay 6 | 7 | -------------------------------------------------------------------------------- /upf_msgs/msg/TypeDeclaration.msg: -------------------------------------------------------------------------------- 1 | ## Declares the existence of a symbolic type. 2 | 3 | # Name of the type that is declared. 4 | string type_name 5 | 6 | # Optional. If the string is non-empty, this is the parent type of `type_name`. 7 | # If set, the parent type must have been previously declared (i.e. should appear earlier in the problem's type declarations. 8 | # feature: HIERARCHICAL_TYPING 9 | string parent_type -------------------------------------------------------------------------------- /upf_msgs/msg/ValidationMetric.msg: -------------------------------------------------------------------------------- 1 | string key 2 | string value -------------------------------------------------------------------------------- /upf_msgs/msg/ValidationRequest.msg: -------------------------------------------------------------------------------- 1 | upf_msgs/Problem problem 2 | upf_msgs/Plan plan 3 | -------------------------------------------------------------------------------- /upf_msgs/msg/ValidationResult.msg: -------------------------------------------------------------------------------- 1 | ## Message sent by the validator. 2 | 3 | # The Plan is valid for the Problem. 4 | uint8 VALID=0 5 | # The Plan is not valid for the Problem. 6 | uint8 INVALID=0 7 | uint8 status 8 | 9 | # Optional. Information given by the engine to the user. 10 | upf_msgs/LogMessage[] log_messages 11 | 12 | # Synthetic description of the engine that generated this message. 13 | string engine 14 | 15 | ValidationMetric[] metrics 16 | 17 | -------------------------------------------------------------------------------- /upf_msgs/msg/Variable.msg: -------------------------------------------------------------------------------- 1 | string name 2 | upf_msgs/TypeDeclaration type -------------------------------------------------------------------------------- /upf_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | upf_msgs 5 | 0.2.0 6 | Messages for the AIPlan4EU Unified Planning 7 | Francisco Martin Rico 8 | Apache License, Version 2.0 9 | 10 | ament_cmake 11 | 12 | builtin_interfaces 13 | rosidl_default_generators 14 | 15 | rosidl_interface_packages 16 | 17 | 18 | ament_cmake 19 | 20 | 21 | -------------------------------------------------------------------------------- /upf_msgs/srv/AddAction.srv: -------------------------------------------------------------------------------- 1 | string problem_name 2 | upf_msgs/Action action 3 | --- 4 | bool success 5 | string message 6 | -------------------------------------------------------------------------------- /upf_msgs/srv/AddFluent.srv: -------------------------------------------------------------------------------- 1 | string problem_name 2 | upf_msgs/Fluent fluent 3 | upf_msgs/Expression default_value 4 | --- 5 | bool success 6 | string message -------------------------------------------------------------------------------- /upf_msgs/srv/AddGoal.srv: -------------------------------------------------------------------------------- 1 | string problem_name 2 | upf_msgs/Goal[<=1] goal 3 | upf_msgs/GoalWithCost[<=1] goal_with_cost 4 | --- 5 | bool success 6 | string message 7 | -------------------------------------------------------------------------------- /upf_msgs/srv/AddObject.srv: -------------------------------------------------------------------------------- 1 | string problem_name 2 | upf_msgs/ObjectDeclaration object 3 | --- 4 | bool success 5 | string message -------------------------------------------------------------------------------- /upf_msgs/srv/GetProblem.srv: -------------------------------------------------------------------------------- 1 | string problem_name 2 | --- 3 | upf_msgs/Problem problem 4 | bool success 5 | string message -------------------------------------------------------------------------------- /upf_msgs/srv/NewProblem.srv: -------------------------------------------------------------------------------- 1 | string problem_name 2 | --- 3 | bool success 4 | string message -------------------------------------------------------------------------------- /upf_msgs/srv/PDDLPlanOneShot.srv: -------------------------------------------------------------------------------- 1 | upf_msgs/PDDLPlanRequest plan_request 2 | --- 3 | upf_msgs/PlanGenerationResult plan_result 4 | bool success 5 | string message 6 | -------------------------------------------------------------------------------- /upf_msgs/srv/PlanOneShot.srv: -------------------------------------------------------------------------------- 1 | upf_msgs/Problem problem 2 | --- 3 | upf_msgs/PlanGenerationResult plan_result 4 | bool success 5 | string message -------------------------------------------------------------------------------- /upf_msgs/srv/SetInitialValue.srv: -------------------------------------------------------------------------------- 1 | string problem_name 2 | upf_msgs/Expression expression 3 | upf_msgs/Expression value 4 | --- 5 | bool success 6 | string message -------------------------------------------------------------------------------- /upf_msgs/srv/SetProblem.srv: -------------------------------------------------------------------------------- 1 | string problem_name 2 | upf_msgs/Problem problem 3 | --- 4 | bool success 5 | string message --------------------------------------------------------------------------------