├── .github └── workflows │ ├── foxy-devel.yaml │ ├── galactic-devel.yaml │ ├── humble-devel.yaml │ ├── iron-devel.yaml │ ├── jazzy-devel.yaml │ ├── kilted-devel.yaml │ └── rolling.yaml ├── .gitignore ├── CODE_OF_CONDUCT.md ├── CONTRIBUTING.md ├── LICENSE ├── README.md ├── codecov.yaml ├── dependency_repos.repos ├── plansys2 ├── CHANGELOG.rst ├── CMakeLists.txt └── package.xml ├── plansys2_bringup ├── CHANGELOG.rst ├── CMakeLists.txt ├── launch │ ├── plansys2_bringup_launch_distributed.py │ └── plansys2_bringup_launch_monolithic.py ├── package.xml ├── params │ └── plansys2_params.yaml └── src │ └── plansys2_node.cpp ├── plansys2_bt_actions ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── include │ └── plansys2_bt_actions │ │ ├── BTAction.hpp │ │ ├── BTActionNode.hpp │ │ ├── BTServiceNode.hpp │ │ ├── BTUtils.hpp │ │ └── JSONUtils.hpp ├── package.xml ├── src │ ├── bt_action_node.cpp │ └── plansys2_bt_actions │ │ └── BTAction.cpp └── test │ ├── CMakeLists.txt │ ├── behavior_tree │ ├── CloseGripper.cpp │ ├── CloseGripper.hpp │ ├── FailureNodes.cpp │ ├── FailureNodes.hpp │ ├── Move.cpp │ ├── Move.hpp │ ├── OpenGripper.cpp │ ├── OpenGripper.hpp │ ├── assemble.xml │ └── transport.xml │ └── unit │ ├── CMakeLists.txt │ └── bt_action_test.cpp ├── plansys2_core ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── include │ └── plansys2_core │ │ ├── Action.hpp │ │ ├── DerivedResolutionGraph.hpp │ │ ├── NodeVariant.hpp │ │ ├── PlanSolverBase.hpp │ │ ├── State.hpp │ │ ├── Types.hpp │ │ └── Utils.hpp ├── package.xml ├── src │ └── plansys2_core │ │ ├── Action.cpp │ │ ├── DerivedResolutionGraph.cpp │ │ ├── PlanSolverBase.cpp │ │ ├── State.cpp │ │ └── Utils.cpp └── test │ ├── CMakeLists.txt │ ├── graph_test.cpp │ ├── node_variant_test.cpp │ ├── pddl │ ├── domain_1_ok.pddl │ ├── domain_2_error.pddl │ ├── domain_simple.pddl │ ├── problem_simple_1.pddl │ ├── problem_simple_2.pddl │ └── problem_simple_3.pddl │ ├── state_test.cpp │ ├── types_test.cpp │ └── utils_test.cpp ├── plansys2_docs ├── Executor_graph.png ├── FAQ.md ├── developer_guide.md ├── plansys2_arch.png ├── plansys2_logo.png ├── plansys2_logo_v2.png ├── plansys2_logo_v2.xcf ├── plansys2_logo_v3.png ├── tutorials.md └── tutorials │ ├── tut_1_terminal.md │ └── tut_2_patrol.md ├── plansys2_domain_expert ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── include │ └── plansys2_domain_expert │ │ ├── DomainExpert.hpp │ │ ├── DomainExpertClient.hpp │ │ ├── DomainExpertInterface.hpp │ │ ├── DomainExpertNode.hpp │ │ └── DomainReader.hpp ├── launch │ └── domain_expert_launch.py ├── package.xml ├── src │ ├── domain_expert_node.cpp │ └── plansys2_domain_expert │ │ ├── DomainExpert.cpp │ │ ├── DomainExpertClient.cpp │ │ ├── DomainExpertNode.cpp │ │ └── DomainReader.cpp └── test │ ├── CMakeLists.txt │ ├── pddl │ ├── domain_1_ok.pddl │ ├── domain_2_error.pddl │ ├── domain_charging.pddl │ ├── domain_combined_processed.pddl │ ├── domain_simple.pddl │ ├── domain_simple_constants.pddl │ ├── domain_simple_constants_processed.pddl │ ├── domain_simple_derived.pddl │ ├── domain_simple_derived_processed.pddl │ ├── domain_simple_ext.pddl │ ├── domain_simple_processed.pddl │ ├── factory.pddl │ ├── factory_processed.pddl │ ├── problem_0.pddl │ ├── problem_charging.pddl │ └── problem_simple_1.pddl │ └── unit │ ├── CMakeLists.txt │ ├── domain_expert_node_test.cpp │ ├── domain_expert_test.cpp │ ├── domain_reader_test.cpp │ └── types_test.cpp ├── plansys2_executor ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── behavior_trees │ ├── plansys2_action_bt.xml │ ├── plansys2_action_bt_with_undo.xml │ ├── plansys2_end_action_bt.xml │ └── plansys2_start_action_bt.xml ├── include │ └── plansys2_executor │ │ ├── ActionExecutor.hpp │ │ ├── ActionExecutorClient.hpp │ │ ├── BTBuilder.hpp │ │ ├── BTUtils.hpp │ │ ├── ComputeBT.hpp │ │ ├── ExecutorClient.hpp │ │ ├── ExecutorNode.hpp │ │ ├── JSONUtils.hpp │ │ ├── behavior_tree │ │ ├── apply_atend_effect_node.hpp │ │ ├── apply_atstart_effect_node.hpp │ │ ├── check_action_node.hpp │ │ ├── check_atend_req_node.hpp │ │ ├── check_overall_req_node.hpp │ │ ├── check_timeout_node.hpp │ │ ├── execute_action_node.hpp │ │ ├── restore_atstart_effect_node.hpp │ │ ├── wait_action_node.hpp │ │ └── wait_atstart_req_node.hpp │ │ └── bt_builder_plugins │ │ ├── sequential_bt_builder.hpp │ │ ├── simple_bt_builder.hpp │ │ └── stn_bt_builder.hpp ├── launch │ ├── compute_bt_launch.py │ └── executor_launch.py ├── package.xml ├── plugins.xml ├── src │ ├── compute_bt.cpp │ ├── executor_node.cpp │ └── plansys2_executor │ │ ├── ActionExecutor.cpp │ │ ├── ActionExecutorClient.cpp │ │ ├── ComputeBT.cpp │ │ ├── ExecutorClient.cpp │ │ ├── ExecutorNode.cpp │ │ ├── behavior_tree │ │ ├── apply_atend_effect_node.cpp │ │ ├── apply_atstart_effect_node.cpp │ │ ├── check_action_node.cpp │ │ ├── check_atend_req_node.cpp │ │ ├── check_overall_req_node.cpp │ │ ├── check_timeout_node.cpp │ │ ├── execute_action_node.cpp │ │ ├── restore_atstart_effect_node.cpp │ │ ├── wait_action_node.cpp │ │ └── wait_atstart_req_node.cpp │ │ └── bt_builder_plugins │ │ ├── sequential_bt_builder.cpp │ │ ├── simple_bt_builder.cpp │ │ └── stn_bt_builder.cpp └── test │ ├── CMakeLists.txt │ ├── pddl │ ├── cooking_domain.pddl │ ├── domain_charging.pddl │ ├── domain_simple.pddl │ ├── domain_simple_2.pddl │ ├── elevator_domain.pddl │ ├── elevator_problem.pddl │ ├── factory.pddl │ ├── factory2.pddl │ ├── factory3.pddl │ ├── factory4.pddl │ ├── problem_factory_1.pddl │ ├── problem_simple_1.pddl │ ├── problem_simple_2.pddl │ ├── problem_simple_3.pddl │ ├── road_trip_domain.pddl │ ├── road_trip_problem.pddl │ ├── simple_example.pddl │ ├── simple_move_example.pddl │ ├── suave_domain.pddl │ └── suave_problem.pddl │ ├── test_behavior_trees │ ├── test_action_timeout_bt.xml │ ├── test_action_timeout_end_bt.xml │ └── test_action_timeout_start_bt.xml │ ├── test_data │ ├── elevator_graph.csv │ └── road_trip_graph.csv │ └── unit │ ├── CMakeLists.txt │ ├── action_execution_test.cpp │ ├── bt_node_test.cpp │ ├── bt_node_test_charging.cpp │ ├── execution_tree_test.cpp │ ├── executor_test.cpp │ ├── sequential_bt_builder_test.cpp │ └── simple_btbuilder_tests.cpp ├── plansys2_lifecycle_manager ├── CHANGELOG.rst ├── CMakeLists.txt ├── include │ └── plansys2_lifecycle_manager │ │ └── lifecycle_manager.hpp ├── package.xml ├── src │ ├── lifecycle_manager_node.cpp │ └── plansys2_lifecycle_manager │ │ └── lifecycle_manager.cpp └── test │ ├── CMakeLists.txt │ └── lf_manager_test.cpp ├── plansys2_msgs ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── action │ └── ExecutePlan.action ├── msg │ ├── Action.msg │ ├── ActionExecution.msg │ ├── ActionExecutionInfo.msg │ ├── ActionPerformerStatus.msg │ ├── Derived.msg │ ├── DurativeAction.msg │ ├── Knowledge.msg │ ├── Node.msg │ ├── Param.msg │ ├── Plan.msg │ ├── PlanArray.msg │ ├── PlanItem.msg │ ├── Problem.msg │ ├── State.msg │ └── Tree.msg ├── package.xml └── srv │ ├── AddProblem.srv │ ├── AddProblemGoal.srv │ ├── AffectNode.srv │ ├── AffectParam.srv │ ├── ClearProblemKnowledge.srv │ ├── ExistNode.srv │ ├── GetDomain.srv │ ├── GetDomainActionDetails.srv │ ├── GetDomainActions.srv │ ├── GetDomainConstants.srv │ ├── GetDomainDerivedPredicateDetails.srv │ ├── GetDomainDurativeActionDetails.srv │ ├── GetDomainName.srv │ ├── GetDomainTypes.srv │ ├── GetNodeDetails.srv │ ├── GetOrderedSubGoals.srv │ ├── GetPlan.srv │ ├── GetPlanArray.srv │ ├── GetProblem.srv │ ├── GetProblemGoal.srv │ ├── GetProblemInstanceDetails.srv │ ├── GetProblemInstances.srv │ ├── GetStates.srv │ ├── IsProblemGoalSatisfied.srv │ ├── RemoveProblemGoal.srv │ └── ValidateDomain.srv ├── plansys2_pddl_parser ├── CHANGELOG.rst ├── CMakeLists.txt ├── LICENSE ├── include │ └── plansys2_pddl_parser │ │ ├── Action.hpp │ │ ├── And.hpp │ │ ├── Basic.hpp │ │ ├── CondIter.hpp │ │ ├── Condition.hpp │ │ ├── Derived.hpp │ │ ├── Domain.hpp │ │ ├── EitherType.hpp │ │ ├── Exists.hpp │ │ ├── Expression.hpp │ │ ├── Forall.hpp │ │ ├── Function.hpp │ │ ├── FunctionModifier.hpp │ │ ├── Ground.hpp │ │ ├── GroundFunc.hpp │ │ ├── Imply.hpp │ │ ├── Instance.hpp │ │ ├── Lifted.hpp │ │ ├── Not.hpp │ │ ├── Oneof.hpp │ │ ├── Or.hpp │ │ ├── ParamCond.hpp │ │ ├── Stringreader.hpp │ │ ├── Task.hpp │ │ ├── TemporalAction.hpp │ │ ├── TokenStruct.hpp │ │ ├── Type.hpp │ │ ├── TypeGround.hpp │ │ ├── Utils.hpp │ │ └── When.hpp ├── package.xml ├── src │ ├── parser.cpp │ └── plansys2_pddl_parser │ │ ├── Action.cpp │ │ ├── And.cpp │ │ ├── Derived.cpp │ │ ├── Exists.cpp │ │ ├── Expression.cpp │ │ ├── Forall.cpp │ │ ├── Function.cpp │ │ ├── FunctionModifier.cpp │ │ ├── Ground.cpp │ │ ├── GroundFunc.cpp │ │ ├── Imply.cpp │ │ ├── Lifted.cpp │ │ ├── Not.cpp │ │ ├── Oneof.cpp │ │ ├── Or.cpp │ │ ├── ParamCond.cpp │ │ ├── TemporalAction.cpp │ │ ├── TypeGround.cpp │ │ ├── Utils.cpp │ │ └── When.cpp └── test │ ├── CMakeLists.txt │ ├── pddl │ ├── dom1.pddl │ ├── dom2.pddl │ ├── prob1.pddl │ └── prob2.pddl │ └── pddl_parser_test.cpp ├── plansys2_planner ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── include │ └── plansys2_planner │ │ ├── PlannerClient.hpp │ │ ├── PlannerInterface.hpp │ │ └── PlannerNode.hpp ├── launch │ └── planner_launch.py ├── package.xml ├── src │ ├── planner_node.cpp │ └── plansys2_planner │ │ ├── PlannerClient.cpp │ │ └── PlannerNode.cpp └── test │ ├── CMakeLists.txt │ ├── pddl │ ├── domain_simple.pddl │ ├── domain_simple_constants.pddl │ ├── problem_simple_1.pddl │ ├── problem_simple_2.pddl │ ├── problem_simple_3.pddl │ ├── problem_simple_constants_1.pddl │ └── problem_simple_constants_2.pddl │ └── unit │ ├── CMakeLists.txt │ └── planner_test.cpp ├── plansys2_popf_plan_solver ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── include │ └── plansys2_popf_plan_solver │ │ └── popf_plan_solver.hpp ├── package.xml ├── plansys2_popf_plan_solver_plugin.xml ├── src │ └── plansys2_popf_plan_solver │ │ └── popf_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 │ └── popf_test.cpp ├── plansys2_problem_expert ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── include │ └── plansys2_problem_expert │ │ ├── ProblemExpert.hpp │ │ ├── ProblemExpertClient.hpp │ │ ├── ProblemExpertInterface.hpp │ │ ├── ProblemExpertNode.hpp │ │ └── Utils.hpp ├── launch │ └── problem_expert_launch.py ├── package.xml ├── src │ ├── plansys2_problem_expert │ │ ├── ProblemExpert.cpp │ │ ├── ProblemExpertClient.cpp │ │ ├── ProblemExpertNode.cpp │ │ └── Utils.cpp │ └── problem_expert_node.cpp └── test │ ├── CMakeLists.txt │ ├── pddl │ ├── domain_charging.pddl │ ├── domain_exists.pddl │ ├── domain_simple.pddl │ ├── domain_simple_constants.pddl │ ├── domain_simple_derived.pddl │ ├── problem_charging.pddl │ ├── problem_empty_domain.pddl │ ├── problem_simple_1.pddl │ ├── problem_simple_constants_1.pddl │ ├── problem_simple_constants_2.pddl │ └── problem_unexpected_syntax.pddl │ └── unit │ ├── CMakeLists.txt │ ├── problem_expert_node_test.cpp │ ├── problem_expert_test.cpp │ └── utils_test.cpp ├── plansys2_support_py ├── CHANGELOG.rst ├── CMakeLists.txt ├── package.xml └── plansys2_support_py │ ├── ActionExecutorClient.py │ └── __init__.py ├── plansys2_terminal ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── include │ └── plansys2_terminal │ │ └── Terminal.hpp ├── package.xml ├── src │ ├── plansys2_terminal │ │ └── Terminal.cpp │ └── terminal_node.cpp └── test │ ├── CMakeLists.txt │ ├── pddl │ ├── commands │ ├── plan │ ├── problem_charging.pddl │ ├── problem_empty_domain.pddl │ └── simple_example.pddl │ └── terminal_test.cpp ├── plansys2_tests ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── include │ └── plansys2_tests │ │ ├── execution_logger.hpp │ │ └── test_action_node.hpp ├── package.xml ├── src │ └── plansys2_tests │ │ ├── execution_logger.cpp │ │ └── test_action_node.cpp ├── test_1 │ ├── CMakeLists.txt │ ├── README.md │ ├── pddl │ │ └── test_1.pddl │ └── test_1.cpp ├── test_2 │ ├── CMakeLists.txt │ ├── README.md │ ├── pddl │ │ ├── problem_2.pddl │ │ └── test_2.pddl │ └── test_2.cpp ├── test_3 │ ├── CMakeLists.txt │ ├── README.md │ ├── pddl │ │ └── test_3.pddl │ └── test_3.cpp └── test_4 │ ├── CMakeLists.txt │ ├── README.md │ ├── pddl │ └── test_4.pddl │ └── test_4.cpp └── plansys2_tools ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── include ├── plansys2_logger │ └── LoggerNode.hpp ├── rqt_plansys2_knowledge │ ├── KnowledgeTree.hpp │ └── RQTKnowledge.hpp ├── rqt_plansys2_performers │ ├── PerformersTree.hpp │ └── RQTPerformers.hpp └── rqt_plansys2_plan │ ├── PlanTree.hpp │ └── RQTPlan.hpp ├── package.xml ├── plugin_knowledge.xml ├── plugin_performers.xml ├── plugin_plan.xml ├── scripts ├── rqt_plansys2_knowledge ├── rqt_plansys2_performers └── rqt_plansys2_plan ├── setup.py └── src ├── logger.cpp ├── plansys2_logger └── LoggerNode.cpp ├── rqt_plansys2_knowledge ├── KnowledgeTree.cpp ├── RQTKnowledge.cpp └── rqt_plansys2_knowledge.ui ├── rqt_plansys2_performers ├── PerformersTree.cpp ├── RQTPerformers.cpp └── rqt_plansys2_performers.ui └── rqt_plansys2_plan ├── PlanTree.cpp ├── RQTPlan.cpp └── rqt_plansys2_plan.ui /.github/workflows/foxy-devel.yaml: -------------------------------------------------------------------------------- 1 | name: foxy-devel 2 | 3 | on: 4 | pull_request: 5 | branches: 6 | - foxy-devel 7 | push: 8 | branches: 9 | - foxy-devel 10 | 11 | jobs: 12 | build-and-test: 13 | runs-on: ${{ matrix.os }} 14 | strategy: 15 | matrix: 16 | os: [ubuntu-20.04] 17 | fail-fast: false 18 | steps: 19 | - name: Install popf deps 20 | run: sudo apt-get install libfl-dev 21 | - name: Fix pip 22 | run: pip3 install empy==3.3.4; pip3 install -U colcon-common-extensions 23 | - name: Setup ROS 2 24 | uses: ros-tooling/setup-ros@0.2.1 25 | with: 26 | required-ros-distributions: foxy 27 | - name: build and test 28 | uses: ros-tooling/action-ros-ci@0.2.1 29 | with: 30 | package-name: plansys2_bringup plansys2_bt_actions plansys2_domain_expert plansys2_executor plansys2_lifecycle_manager plansys2_msgs plansys2_pddl_parser plansys2_planner plansys2_popf_plan_solver plansys2_problem_expert plansys2_terminal plansys2_tests 31 | target-ros2-distro: foxy 32 | colcon-defaults: | 33 | { 34 | "test": { 35 | "parallel-workers" : 1 36 | } 37 | } 38 | vcs-repo-file-url: "" 39 | colcon-mixin-name: coverage-gcc 40 | colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml 41 | - name: Codecov 42 | uses: codecov/codecov-action@v1.1.0 43 | with: 44 | file: ros_ws/lcov/total_coverage.info 45 | flags: unittests 46 | name: codecov-umbrella 47 | # yml: ./codecov.yml 48 | fail_ci_if_error: false 49 | -------------------------------------------------------------------------------- /.github/workflows/galactic-devel.yaml: -------------------------------------------------------------------------------- 1 | name: galactic-devel 2 | 3 | on: 4 | pull_request: 5 | branches: 6 | - galactic-devel 7 | push: 8 | branches: 9 | - galactic-devel 10 | 11 | jobs: 12 | build-and-test: 13 | runs-on: ${{ matrix.os }} 14 | strategy: 15 | matrix: 16 | os: [ubuntu-20.04] 17 | fail-fast: false 18 | steps: 19 | - name: Install popf deps 20 | run: sudo apt-get install libfl-dev 21 | - name: Fix pip 22 | run: python3 -m pip3 install pip3 --upgrade; pip3 install empy==3.3.4; pip3 install -U colcon-common-extensions; pip3 install pyopenssl --upgrade 23 | - name: Setup ROS 2 24 | uses: ros-tooling/setup-ros@0.2.1 25 | with: 26 | required-ros-distributions: galactic 27 | - name: build and test 28 | uses: ros-tooling/action-ros-ci@0.2.1 29 | with: 30 | package-name: plansys2_bringup plansys2_bt_actions plansys2_domain_expert plansys2_executor plansys2_lifecycle_manager plansys2_msgs plansys2_pddl_parser plansys2_planner plansys2_popf_plan_solver plansys2_problem_expert plansys2_terminal plansys2_tests plansys2_tools 31 | target-ros2-distro: galactic 32 | colcon-defaults: | 33 | { 34 | "test": { 35 | "parallel-workers" : 1 36 | } 37 | } 38 | vcs-repo-file-url: https://raw.githubusercontent.com/IntelligentRoboticsLabs/ros2_planning_system/galactic-devel/dependency_repos.repos 39 | colcon-mixin-name: coverage-gcc 40 | colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml 41 | - name: Codecov 42 | uses: codecov/codecov-action@v1.1.0 43 | with: 44 | file: ros_ws/lcov/total_coverage.info 45 | flags: unittests 46 | name: codecov-umbrella 47 | # yml: ./codecov.yml 48 | fail_ci_if_error: false 49 | -------------------------------------------------------------------------------- /.github/workflows/humble-devel.yaml: -------------------------------------------------------------------------------- 1 | name: humble-devel 2 | 3 | on: 4 | pull_request: 5 | branches: 6 | - humble-devel 7 | push: 8 | branches: 9 | - humble-devel 10 | 11 | jobs: 12 | build-and-test: 13 | runs-on: ${{ matrix.os }} 14 | strategy: 15 | matrix: 16 | os: [ubuntu-22.04] 17 | fail-fast: false 18 | steps: 19 | - name: Install popf deps 20 | run: sudo apt-get install libfl-dev 21 | - name: Fix pip 22 | run: pip3 install empy==3.3.4; pip3 install -U colcon-common-extensions 23 | - name: Setup ROS 2 24 | uses: ros-tooling/setup-ros@0.3.3 25 | with: 26 | required-ros-distributions: humble 27 | - name: build and test 28 | uses: ros-tooling/action-ros-ci@0.2.5 29 | with: 30 | package-name: plansys2_bringup plansys2_bt_actions plansys2_domain_expert plansys2_executor plansys2_lifecycle_manager plansys2_msgs plansys2_pddl_parser plansys2_planner plansys2_popf_plan_solver plansys2_problem_expert plansys2_terminal plansys2_tests plansys2_tools 31 | target-ros2-distro: humble 32 | colcon-defaults: | 33 | { 34 | "test": { 35 | "parallel-workers" : 1 36 | } 37 | } 38 | colcon-mixin-name: coverage-gcc 39 | colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml 40 | - name: Codecov 41 | uses: codecov/codecov-action@v1.2.1 42 | with: 43 | file: ros_ws/lcov/total_coverage.info 44 | flags: unittests 45 | name: codecov-umbrella 46 | # yml: ./codecov.yml 47 | fail_ci_if_error: false 48 | -------------------------------------------------------------------------------- /.github/workflows/iron-devel.yaml: -------------------------------------------------------------------------------- 1 | name: iron-devel 2 | 3 | on: 4 | pull_request: 5 | branches: 6 | - iron-devel 7 | push: 8 | branches: 9 | - iron-devel 10 | 11 | jobs: 12 | build-and-test: 13 | runs-on: ${{ matrix.os }} 14 | strategy: 15 | matrix: 16 | os: [ubuntu-22.04] 17 | fail-fast: false 18 | steps: 19 | - name: Install popf deps 20 | run: sudo apt-get install libfl-dev 21 | - name: Fix pip 22 | run: python3 -m pip3 install pip3 --upgrade; pip3 install empy==3.3.4; pip3 install -U colcon-common-extensions; pip3 install pyopenssl --upgrade 23 | - name: Setup ROS 2 24 | uses: ros-tooling/setup-ros@0.7.0 25 | with: 26 | required-ros-distributions: iron 27 | - name: build and test 28 | uses: ros-tooling/action-ros-ci@0.3.3 29 | with: 30 | package-name: plansys2_bringup plansys2_bt_actions plansys2_domain_expert plansys2_executor plansys2_lifecycle_manager plansys2_msgs plansys2_pddl_parser plansys2_planner plansys2_popf_plan_solver plansys2_problem_expert plansys2_terminal plansys2_tests plansys2_tools 31 | target-ros2-distro: iron 32 | colcon-defaults: | 33 | { 34 | "test": { 35 | "parallel-workers" : 1 36 | } 37 | } 38 | colcon-mixin-name: coverage-gcc 39 | colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml 40 | - name: Codecov 41 | uses: codecov/codecov-action@v1.2.1 42 | with: 43 | file: ros_ws/lcov/total_coverage.info 44 | flags: unittests 45 | name: codecov-umbrella 46 | # yml: ./codecov.yml 47 | fail_ci_if_error: false 48 | -------------------------------------------------------------------------------- /.github/workflows/jazzy-devel.yaml: -------------------------------------------------------------------------------- 1 | name: jazzy-devel 2 | 3 | on: 4 | pull_request: 5 | branches: 6 | - jazzy-devel 7 | push: 8 | branches: 9 | - jazzy-devel 10 | schedule: 11 | - cron: '0 0 * * 6' 12 | jobs: 13 | build-and-test: 14 | runs-on: ${{ matrix.os }} 15 | strategy: 16 | matrix: 17 | os: [ubuntu-24.04] 18 | fail-fast: false 19 | steps: 20 | - name: Install popf deps 21 | run: sudo apt-get install libfl-dev 22 | - uses: actions/checkout@v4 23 | with: 24 | ref: jazzy-devel 25 | - name: Setup ROS 2 26 | uses: ros-tooling/setup-ros@0.7.9 27 | with: 28 | required-ros-distributions: jazzy 29 | - name: Install BT.CPP v4 and test_msgs (provisional Fix) 30 | run: sudo apt-get install ros-jazzy-behaviortree-cpp ros-jazzy-test-msgs 31 | - name: build and test 32 | uses: ros-tooling/action-ros-ci@0.3.13 33 | with: 34 | package-name: plansys2_bringup plansys2_bt_actions plansys2_domain_expert plansys2_executor plansys2_lifecycle_manager plansys2_msgs plansys2_pddl_parser plansys2_planner plansys2_popf_plan_solver plansys2_problem_expert plansys2_terminal plansys2_tests plansys2_tools 35 | ref: jazzy-devel 36 | target-ros2-distro: jazzy 37 | colcon-defaults: | 38 | { 39 | "test": { 40 | "parallel-workers" : 1 41 | } 42 | } 43 | vcs-repo-file-url: ${GITHUB_WORKSPACE}/dependency_repos.repos 44 | colcon-mixin-name: coverage-gcc 45 | colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml 46 | - name: Codecov 47 | uses: codecov/codecov-action@v1.2.1 48 | with: 49 | file: ros_ws/lcov/total_coverage.info 50 | flags: unittests 51 | name: codecov-umbrella 52 | # yml: ./codecov.yml 53 | fail_ci_if_error: false 54 | -------------------------------------------------------------------------------- /.github/workflows/kilted-devel.yaml: -------------------------------------------------------------------------------- 1 | name: kilted-devel 2 | 3 | on: 4 | pull_request: 5 | branches: 6 | - kilted-devel 7 | push: 8 | branches: 9 | - kilted-devel 10 | schedule: 11 | - cron: '0 0 * * 6' 12 | jobs: 13 | build-and-test: 14 | runs-on: ${{ matrix.os }} 15 | strategy: 16 | matrix: 17 | os: [ubuntu-24.04] 18 | fail-fast: false 19 | steps: 20 | - name: Install popf deps 21 | run: sudo apt-get install libfl-dev 22 | - uses: actions/checkout@v4.2.2 23 | - name: Setup ROS 2 24 | uses: ros-tooling/setup-ros@0.7.13 25 | with: 26 | required-ros-distributions: kilted-devel 27 | - name: build and test 28 | uses: ros-tooling/action-ros-ci@0.4.3 29 | with: 30 | package-name: plansys2_bringup plansys2_bt_actions plansys2_domain_expert plansys2_executor plansys2_lifecycle_manager plansys2_msgs plansys2_pddl_parser plansys2_planner plansys2_popf_plan_solver plansys2_problem_expert plansys2_terminal plansys2_tests plansys2_tools 31 | target-ros2-distro: kilted-devel 32 | colcon-defaults: | 33 | { 34 | "test": { 35 | "parallel-workers" : 1 36 | } 37 | } 38 | vcs-repo-file-url: ${GITHUB_WORKSPACE}/dependency_repos.repos 39 | colcon-mixin-name: coverage-gcc 40 | colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml 41 | - name: Codecov 42 | uses: codecov/codecov-action@v1.2.1 43 | with: 44 | file: ros_ws/lcov/total_coverage.info 45 | flags: unittests 46 | name: codecov-umbrella 47 | # yml: ./codecov.yml 48 | -------------------------------------------------------------------------------- /.github/workflows/rolling.yaml: -------------------------------------------------------------------------------- 1 | name: rolling 2 | 3 | on: 4 | pull_request: 5 | branches: 6 | - rolling 7 | push: 8 | branches: 9 | - rolling 10 | schedule: 11 | - cron: '0 0 * * 6' 12 | jobs: 13 | build-and-test: 14 | runs-on: ${{ matrix.os }} 15 | strategy: 16 | matrix: 17 | os: [ubuntu-24.04] 18 | fail-fast: false 19 | steps: 20 | - name: Install popf deps 21 | run: sudo apt-get install libfl-dev 22 | - uses: actions/checkout@v4.2.2 23 | - name: Setup ROS 2 24 | uses: ros-tooling/setup-ros@0.7.13 25 | with: 26 | required-ros-distributions: rolling 27 | - name: build and test 28 | uses: ros-tooling/action-ros-ci@0.4.3 29 | with: 30 | package-name: plansys2_bringup plansys2_bt_actions plansys2_domain_expert plansys2_executor plansys2_lifecycle_manager plansys2_msgs plansys2_pddl_parser plansys2_planner plansys2_popf_plan_solver plansys2_problem_expert plansys2_terminal plansys2_tests plansys2_tools 31 | target-ros2-distro: rolling 32 | colcon-defaults: | 33 | { 34 | "test": { 35 | "parallel-workers" : 1 36 | } 37 | } 38 | vcs-repo-file-url: ${GITHUB_WORKSPACE}/dependency_repos.repos 39 | colcon-mixin-name: coverage-gcc 40 | colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml 41 | - name: Codecov 42 | uses: codecov/codecov-action@v1.2.1 43 | with: 44 | file: ros_ws/lcov/total_coverage.info 45 | flags: unittests 46 | name: codecov-umbrella 47 | # yml: ./codecov.yml 48 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | /.vscode/** 2 | *.swp 3 | -------------------------------------------------------------------------------- /codecov.yaml: -------------------------------------------------------------------------------- 1 | fixes: 2 | - "ros_ws/src/*/ros2_planning_system/::" 3 | ignore: 4 | - "ros_ws/build" 5 | - "ros_ws/*/**/test/*" 6 | - "ros_ws/*/**/**/test/*" 7 | -------------------------------------------------------------------------------- /dependency_repos.repos: -------------------------------------------------------------------------------- 1 | repositories: 2 | cascade_lifecycle: 3 | type: git 4 | url: https://github.com/fmrico/cascade_lifecycle.git 5 | version: rolling 6 | popf: 7 | type: git 8 | url: https://github.com/fmrico/popf.git 9 | version: ros2 10 | behavior_tree_cpp: 11 | type: git 12 | url: https://github.com/BehaviorTree/BehaviorTree.CPP.git 13 | version: 4.7.0 14 | 15 | -------------------------------------------------------------------------------- /plansys2/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package plansys2 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 3.0.0 (2025-06-06) 6 | ------------------ 7 | * Merge pull request `#366 `_ from fmrico/add_exports 8 | Modify export target. Rename metapackage 9 | * Modify export target. Rename metapackage 10 | * Contributors: Francisco Martín Rico 11 | -------------------------------------------------------------------------------- /plansys2/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(plansys2) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | 6 | ament_package() 7 | -------------------------------------------------------------------------------- /plansys2/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | plansys2 5 | 3.0.0 6 | 7 | ROS2 Planning System 8 | 9 | Francisco Martin Rico 10 | 11 | Apache License, Version 2.0 12 | 13 | ament_cmake 14 | 15 | plansys2_bringup 16 | plansys2_bt_actions 17 | plansys2_core 18 | plansys2_domain_expert 19 | plansys2_executor 20 | plansys2_lifecycle_manager 21 | plansys2_msgs 22 | plansys2_pddl_parser 23 | plansys2_planner 24 | plansys2_popf_plan_solver 25 | plansys2_problem_expert 26 | plansys2_support_py 27 | plansys2_terminal 28 | plansys2_tools 29 | 30 | 31 | ament_cmake 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /plansys2_bringup/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | project(plansys2_bringup) 4 | 5 | find_package(ament_cmake REQUIRED) 6 | find_package(rclcpp REQUIRED) 7 | find_package(plansys2_domain_expert REQUIRED) 8 | find_package(plansys2_problem_expert REQUIRED) 9 | find_package(plansys2_planner REQUIRED) 10 | find_package(plansys2_executor REQUIRED) 11 | find_package(plansys2_lifecycle_manager REQUIRED) 12 | 13 | add_executable(plansys2_node 14 | src/plansys2_node.cpp 15 | ) 16 | target_link_libraries(plansys2_node 17 | PRIVATE 18 | rclcpp::rclcpp 19 | plansys2_domain_expert::plansys2_domain_expert 20 | plansys2_problem_expert::plansys2_problem_expert 21 | plansys2_planner::plansys2_planner 22 | plansys2_executor::plansys2_executor 23 | plansys2_lifecycle_manager::plansys2_lifecycle_manager 24 | ) 25 | target_compile_definitions(plansys2_node PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") 26 | 27 | if(BUILD_TESTING) 28 | find_package(ament_lint_auto REQUIRED) 29 | ament_lint_auto_find_test_dependencies() 30 | endif() 31 | 32 | install(DIRECTORY launch params 33 | DESTINATION share/${PROJECT_NAME} 34 | ) 35 | install(TARGETS plansys2_node 36 | RUNTIME DESTINATION lib/${PROJECT_NAME} 37 | ) 38 | 39 | ament_export_dependencies( 40 | rclcpp 41 | plansys2_domain_expert 42 | plansys2_problem_expert 43 | plansys2_planner 44 | plansys2_executor 45 | plansys2_lifecycle_manager) 46 | ament_package() 47 | -------------------------------------------------------------------------------- /plansys2_bringup/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | plansys2_bringup 5 | 3.0.0 6 | 7 | Bringup scripts and configurations for the ROS2 Planning System 8 | 9 | Francisco Martin Rico 10 | 11 | Apache License, Version 2.0 12 | 13 | ament_cmake 14 | 15 | rclcpp 16 | plansys2_domain_expert 17 | plansys2_problem_expert 18 | plansys2_planner 19 | plansys2_executor 20 | plansys2_lifecycle_manager 21 | 22 | launch_ros 23 | 24 | ament_lint_common 25 | ament_lint_auto 26 | launch 27 | launch_testing 28 | 29 | 30 | ament_cmake 31 | 32 | 33 | -------------------------------------------------------------------------------- /plansys2_bringup/params/plansys2_params.yaml: -------------------------------------------------------------------------------- 1 | plansys2: 2 | ros__parameters: 3 | use_real_time: false 4 | planner_client: 5 | ros__parameters: 6 | plan_solver_timeout: 15.0 7 | planner: 8 | ros__parameters: 9 | plan_solver_timeout: 15.0 10 | plan_solver_plugins: ["POPF"] 11 | POPF: 12 | plugin: "plansys2/POPFPlanSolver" 13 | TFD: 14 | plugin: "plansys2/TFDPlanSolver" 15 | executor: 16 | ros__parameters: 17 | bt_builder_plugin: "SimpleBTBuilder" 18 | # bt_builder_plugin: "STNBTBuilder" 19 | -------------------------------------------------------------------------------- /plansys2_bt_actions/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | plansys2_bt_actions 5 | 3.0.0 6 | 7 | This package contains the Problem Expert module for the ROS2 Planning System 8 | 9 | Francisco Martin Rico 10 | 11 | Apache License, Version 2.0 12 | 13 | ament_cmake 14 | 15 | rclcpp 16 | rclcpp_action 17 | rclcpp_lifecycle 18 | plansys2_executor 19 | behaviortree_cpp 20 | lifecycle_msgs 21 | std_msgs 22 | 23 | ament_lint_common 24 | ament_lint_auto 25 | ament_cmake_gtest 26 | plansys2_msgs 27 | test_msgs 28 | geometry_msgs 29 | tf2_geometry_msgs 30 | 31 | 32 | ament_cmake 33 | 34 | 35 | -------------------------------------------------------------------------------- /plansys2_bt_actions/src/bt_action_node.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 "plansys2_bt_actions/BTAction.hpp" 19 | #include "lifecycle_msgs/msg/transition.hpp" 20 | #include "rclcpp/rclcpp.hpp" 21 | 22 | 23 | using namespace std::chrono_literals; 24 | 25 | int main(int argc, char ** argv) 26 | { 27 | rclcpp::init(argc, argv); 28 | 29 | auto action_node = std::make_shared("default"); 30 | 31 | action_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); 32 | 33 | rclcpp::spin(action_node->get_node_base_interface()); 34 | 35 | rclcpp::shutdown(); 36 | 37 | return 0; 38 | } 39 | -------------------------------------------------------------------------------- /plansys2_bt_actions/test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory(unit) 2 | -------------------------------------------------------------------------------- /plansys2_bt_actions/test/behavior_tree/CloseGripper.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 "CloseGripper.hpp" 19 | 20 | #include "behaviortree_cpp/behavior_tree.h" 21 | 22 | namespace plansys2_bt_tests 23 | { 24 | 25 | CloseGripper::CloseGripper( 26 | const std::string & xml_tag_name, 27 | const BT::NodeConfig & conf) 28 | : BT::ActionNodeBase(xml_tag_name, conf), counter_(0) 29 | { 30 | } 31 | 32 | void 33 | CloseGripper::halt() 34 | { 35 | std::cout << "CloseGripper halt" << std::endl; 36 | } 37 | 38 | BT::NodeStatus 39 | CloseGripper::tick() 40 | { 41 | std::cout << "CloseGripper tick " << counter_ << std::endl; 42 | 43 | if (counter_++ < 5) { 44 | return BT::NodeStatus::RUNNING; 45 | } else { 46 | counter_ = 0; 47 | return BT::NodeStatus::SUCCESS; 48 | } 49 | } 50 | 51 | } // namespace plansys2_bt_tests 52 | 53 | #include "behaviortree_cpp/bt_factory.h" 54 | BT_REGISTER_NODES(factory) 55 | { 56 | factory.registerNodeType("CloseGripper"); 57 | } 58 | -------------------------------------------------------------------------------- /plansys2_bt_actions/test/behavior_tree/CloseGripper.hpp: -------------------------------------------------------------------------------- 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 | #ifndef BEHAVIOR_TREE__CLOSEGRIPPER_HPP_ 16 | #define BEHAVIOR_TREE__CLOSEGRIPPER_HPP_ 17 | 18 | #include 19 | 20 | #include "behaviortree_cpp/behavior_tree.h" 21 | #include "behaviortree_cpp/bt_factory.h" 22 | 23 | namespace plansys2_bt_tests 24 | { 25 | 26 | class CloseGripper : public BT::ActionNodeBase 27 | { 28 | public: 29 | explicit CloseGripper( 30 | const std::string & xml_tag_name, 31 | const BT::NodeConfig & conf); 32 | 33 | void halt(); 34 | BT::NodeStatus tick(); 35 | 36 | static BT::PortsList providedPorts() 37 | { 38 | return BT::PortsList({}); 39 | } 40 | 41 | private: 42 | int counter_; 43 | }; 44 | 45 | } // namespace plansys2_bt_tests 46 | 47 | #endif // BEHAVIOR_TREE__CLOSEGRIPPER_HPP_ 48 | -------------------------------------------------------------------------------- /plansys2_bt_actions/test/behavior_tree/FailureNodes.cpp: -------------------------------------------------------------------------------- 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 | #include 16 | #include 17 | 18 | #include "behaviortree_cpp/bt_factory.h" 19 | 20 | #include "FailureNodes.hpp" 21 | 22 | BT_REGISTER_NODES(factory) 23 | { 24 | BT::NodeBuilder builder = 25 | [](const std::string & name, const BT::NodeConfig & config) 26 | { 27 | return std::make_unique( 28 | name, "move", config); 29 | }; 30 | 31 | factory.registerBuilder( 32 | "OnTickFail", builder); 33 | 34 | builder = 35 | [](const std::string & name, const BT::NodeConfig & config) 36 | { 37 | return std::make_unique( 38 | name, "move", config); 39 | }; 40 | 41 | factory.registerBuilder( 42 | "OnFeedbackFail", builder); 43 | } 44 | -------------------------------------------------------------------------------- /plansys2_bt_actions/test/behavior_tree/Move.hpp: -------------------------------------------------------------------------------- 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 | #ifndef BEHAVIOR_TREE__MOVE_HPP_ 16 | #define BEHAVIOR_TREE__MOVE_HPP_ 17 | 18 | #include 19 | #include 20 | 21 | #include "geometry_msgs/msg/pose.hpp" 22 | #include "test_msgs/action/fibonacci.hpp" 23 | 24 | #include "plansys2_bt_actions/BTActionNode.hpp" 25 | #include "behaviortree_cpp/behavior_tree.h" 26 | #include "behaviortree_cpp/bt_factory.h" 27 | 28 | namespace plansys2_bt_tests 29 | { 30 | 31 | class Move : public plansys2::BtActionNode 32 | { 33 | public: 34 | explicit Move( 35 | const std::string & xml_tag_name, 36 | const std::string & action_name, 37 | const BT::NodeConfig & conf); 38 | 39 | BT::NodeStatus on_tick() override; 40 | BT::NodeStatus on_success() override; 41 | 42 | static BT::PortsList providedPorts() 43 | { 44 | return { 45 | BT::InputPort("goal"), 46 | BT::OutputPort("goal_reached"), 47 | }; 48 | } 49 | 50 | private: 51 | int goal_reached_; 52 | std::map waypoints_; 53 | }; 54 | 55 | } // namespace plansys2_bt_tests 56 | 57 | #endif // BEHAVIOR_TREE__MOVE_HPP_ 58 | -------------------------------------------------------------------------------- /plansys2_bt_actions/test/behavior_tree/OpenGripper.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 "OpenGripper.hpp" 19 | 20 | #include "behaviortree_cpp/behavior_tree.h" 21 | 22 | namespace plansys2_bt_tests 23 | { 24 | 25 | OpenGripper::OpenGripper( 26 | const std::string & xml_tag_name, 27 | const BT::NodeConfig & conf) 28 | : BT::ActionNodeBase(xml_tag_name, conf), counter_(0) 29 | { 30 | } 31 | 32 | void 33 | OpenGripper::halt() 34 | { 35 | std::cout << "OpenGripper halt" << std::endl; 36 | } 37 | 38 | BT::NodeStatus 39 | OpenGripper::tick() 40 | { 41 | std::cout << "OpenGripper tick " << counter_ << std::endl; 42 | 43 | if (counter_++ < 5) { 44 | return BT::NodeStatus::RUNNING; 45 | } else { 46 | counter_ = 0; 47 | return BT::NodeStatus::SUCCESS; 48 | } 49 | } 50 | 51 | } // namespace plansys2_bt_tests 52 | 53 | #include "behaviortree_cpp/bt_factory.h" 54 | BT_REGISTER_NODES(factory) 55 | { 56 | factory.registerNodeType("OpenGripper"); 57 | } 58 | -------------------------------------------------------------------------------- /plansys2_bt_actions/test/behavior_tree/OpenGripper.hpp: -------------------------------------------------------------------------------- 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 | #ifndef BEHAVIOR_TREE__OPENGRIPPER_HPP_ 16 | #define BEHAVIOR_TREE__OPENGRIPPER_HPP_ 17 | 18 | #include 19 | 20 | #include "behaviortree_cpp/behavior_tree.h" 21 | #include "behaviortree_cpp/bt_factory.h" 22 | 23 | namespace plansys2_bt_tests 24 | { 25 | 26 | class OpenGripper : public BT::ActionNodeBase 27 | { 28 | public: 29 | explicit OpenGripper( 30 | const std::string & xml_tag_name, 31 | const BT::NodeConfig & conf); 32 | 33 | void halt(); 34 | BT::NodeStatus tick(); 35 | 36 | static BT::PortsList providedPorts() 37 | { 38 | return BT::PortsList({}); 39 | } 40 | 41 | private: 42 | int counter_; 43 | }; 44 | 45 | } // namespace plansys2_bt_tests 46 | 47 | #endif // BEHAVIOR_TREE__OPENGRIPPER_HPP_ 48 | -------------------------------------------------------------------------------- /plansys2_bt_actions/test/behavior_tree/assemble.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /plansys2_bt_actions/test/behavior_tree/transport.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /plansys2_bt_actions/test/unit/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(plansys2_close_gripper_bt_node SHARED 2 | ../behavior_tree/CloseGripper.cpp 3 | ) 4 | list(APPEND plugin_libs plansys2_close_gripper_bt_node) 5 | 6 | add_library(plansys2_open_gripper_bt_node SHARED 7 | ../behavior_tree/OpenGripper.cpp 8 | ) 9 | list(APPEND plugin_libs plansys2_open_gripper_bt_node) 10 | 11 | add_library(plansys2_move_bt_test_node SHARED ../behavior_tree/Move.cpp) 12 | list(APPEND plugin_libs plansys2_move_bt_test_node) 13 | 14 | add_library(plansys2_failure_test_nodes SHARED 15 | ../behavior_tree/FailureNodes.cpp 16 | ) 17 | list(APPEND plugin_libs plansys2_failure_test_nodes) 18 | 19 | foreach(bt_plugin ${plugin_libs}) 20 | target_include_directories(${bt_plugin} 21 | PRIVATE 22 | "$") 23 | target_link_libraries(${bt_plugin} 24 | behaviortree_cpp::behaviortree_cpp 25 | ${geometry_msgs_TARGETS} 26 | tf2_geometry_msgs::tf2_geometry_msgs 27 | rclcpp::rclcpp 28 | rclcpp_action::rclcpp_action 29 | rclcpp_lifecycle::rclcpp_lifecycle 30 | ${test_msgs_TARGETS} 31 | ) 32 | target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT) 33 | endforeach() 34 | 35 | ament_add_gtest(bt_action_test bt_action_test.cpp) 36 | target_link_libraries(bt_action_test 37 | ${PROJECT_NAME} 38 | ${geometry_msgs_TARGETS} 39 | ${lifecycle_msgs_TARGETS} 40 | ${test_msgs_TARGETS} 41 | ) 42 | 43 | install(DIRECTORY ../behavior_tree 44 | DESTINATION share/${PROJECT_NAME}/test 45 | ) 46 | 47 | install(TARGETS ${plugin_libs} 48 | ARCHIVE DESTINATION lib 49 | LIBRARY DESTINATION lib 50 | RUNTIME DESTINATION bin 51 | ) 52 | -------------------------------------------------------------------------------- /plansys2_core/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | project(plansys2_core) 4 | 5 | find_package(ament_cmake REQUIRED) 6 | find_package(rclcpp REQUIRED) 7 | find_package(rclcpp_lifecycle REQUIRED) 8 | find_package(plansys2_pddl_parser REQUIRED) 9 | find_package(plansys2_msgs REQUIRED) 10 | 11 | add_library(${PROJECT_NAME} SHARED 12 | src/plansys2_core/Action.cpp 13 | src/plansys2_core/DerivedResolutionGraph.cpp 14 | src/plansys2_core/PlanSolverBase.cpp 15 | src/plansys2_core/State.cpp 16 | src/plansys2_core/Utils.cpp 17 | ) 18 | target_include_directories(${PROJECT_NAME} PUBLIC 19 | "$" 20 | "$" 21 | "$") 22 | target_link_libraries(${PROJECT_NAME} 23 | PUBLIC 24 | rclcpp::rclcpp 25 | rclcpp_lifecycle::rclcpp_lifecycle 26 | plansys2_pddl_parser::plansys2_pddl_parser 27 | ${plansys2_msgs_TARGETS} 28 | ) 29 | 30 | install(DIRECTORY include/ 31 | DESTINATION include/${PROJECT_NAME} 32 | ) 33 | 34 | install(TARGETS ${PROJECT_NAME} 35 | EXPORT export_${PROJECT_NAME} 36 | ARCHIVE DESTINATION lib 37 | LIBRARY DESTINATION lib 38 | RUNTIME DESTINATION bin 39 | ) 40 | 41 | if(BUILD_TESTING) 42 | find_package(ament_lint_auto REQUIRED) 43 | ament_lint_auto_find_test_dependencies() 44 | find_package(ament_cmake_gtest REQUIRED) 45 | find_package(ament_index_cpp REQUIRED) 46 | add_subdirectory(test) 47 | endif() 48 | 49 | ament_export_include_directories("include/${PROJECT_NAME}") 50 | ament_export_libraries(${PROJECT_NAME}) 51 | ament_export_targets(export_${PROJECT_NAME}) 52 | ament_export_dependencies( 53 | rclcpp 54 | rclcpp_lifecycle 55 | plansys2_pddl_parser 56 | plansys2_msgs 57 | ) 58 | 59 | ament_package() 60 | -------------------------------------------------------------------------------- /plansys2_core/README.md: -------------------------------------------------------------------------------- 1 | # PlanSys2 Core 2 | 3 | This package hosts the abstract interface (virtual base classes) for plugins to be used with the following: 4 | - Planner (e.g., `popf`, `downward`) 5 | -------------------------------------------------------------------------------- /plansys2_core/include/plansys2_core/Utils.hpp: -------------------------------------------------------------------------------- 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 | #ifndef PLANSYS2_CORE__UTILS_HPP_ 16 | #define PLANSYS2_CORE__UTILS_HPP_ 17 | 18 | #include 19 | #include 20 | 21 | namespace plansys2 22 | { 23 | 24 | /** 25 | * @brief Splits a string into tokens based on a delimiter. 26 | * @param[in] string The original string to tokenize. 27 | * @param[in] delim The delimiter used to split the string. 28 | * @return std::vector Vector containing the resulting tokens. 29 | */ 30 | std::vector tokenize(const std::string & string, const std::string & delim); 31 | 32 | /** 33 | * @brief Get a substring without empty lines. 34 | * @param[in] string The original string. 35 | * @param[in] init_pos The first character in the original string. 36 | * @param[in] end_pos The last character in the original string. 37 | * @return std::string A substring without empty lines. 38 | */ 39 | std::string substr_without_empty_lines( 40 | std::string string, std::size_t init_pos, std::size_t end_pos); 41 | 42 | /** 43 | * @brief Remove the comments from a PDDL string. 44 | * @param[in] pddl A PDDL string. 45 | * @return std::string The PDDL string without comments. 46 | */ 47 | std::string remove_comments(const std::string & pddl); 48 | 49 | } // namespace plansys2 50 | 51 | #endif // PLANSYS2_CORE__UTILS_HPP_ 52 | -------------------------------------------------------------------------------- /plansys2_core/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | plansys2_core 5 | 3.0.0 6 | 7 | This package contains the PDDL-based core for the ROS2 Planning System 8 | 9 | Francisco Martin Rico 10 | 11 | Apache License, Version 2.0 12 | 13 | ament_cmake 14 | 15 | rclcpp 16 | rclcpp_lifecycle 17 | plansys2_pddl_parser 18 | plansys2_msgs 19 | 20 | ament_lint_common 21 | ament_lint_auto 22 | ament_cmake_gtest 23 | 24 | 25 | ament_cmake 26 | 27 | 28 | -------------------------------------------------------------------------------- /plansys2_core/test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ament_add_gtest(utils_test utils_test.cpp) 2 | target_link_libraries(utils_test 3 | ${PROJECT_NAME} 4 | ament_index_cpp::ament_index_cpp 5 | ) 6 | 7 | ament_add_gtest(types_test types_test.cpp) 8 | target_link_libraries(types_test ${PROJECT_NAME}) 9 | 10 | ament_add_gtest(node_variant_test node_variant_test.cpp) 11 | target_link_libraries(node_variant_test ${PROJECT_NAME}) 12 | 13 | ament_add_gtest(graph_test graph_test.cpp) 14 | target_link_libraries(graph_test ${PROJECT_NAME}) 15 | 16 | ament_add_gtest(state_test state_test.cpp) 17 | target_link_libraries(state_test ${PROJECT_NAME}) 18 | 19 | set(TEST_PDDL_DIR ${CMAKE_CURRENT_SOURCE_DIR}/pddl) 20 | 21 | install(DIRECTORY 22 | ${TEST_PDDL_DIR} 23 | DESTINATION share/${PROJECT_NAME} 24 | ) 25 | -------------------------------------------------------------------------------- /plansys2_core/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_core/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_at leia bedroom) 19 | ) 20 | ) 21 | ) 22 | -------------------------------------------------------------------------------- /plansys2_core/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_core/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_docs/Executor_graph.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PlanSys2/ros2_planning_system/b30a2fce581b898a4ed1a2c92e6a7fb8b40b9f14/plansys2_docs/Executor_graph.png -------------------------------------------------------------------------------- /plansys2_docs/FAQ.md: -------------------------------------------------------------------------------- 1 | # FAQ 2 | 3 | ## 1. How to debug errors in PDDL? 4 | 5 | Working with PDDL is difficult. If the PDDL domain is not well designed, or predicates or instances are missing, it is impossible to generate a plan. Plansys2 will notify you, but it is difficult to debug and solve the problem. 6 | 7 | When it is required to generate a plan, Plansys2 generates a file with the domain, `/tmp/${namespace}/domain.pddl` and another with the problem `/tmp/${namespace}/domain.pddl`. In addition, the output of the plan solver (popf) is saved in /tmp/${namespace}/plan.pddl. 8 | 9 | It is possible to execute the plan solver in isolation using the command: 10 | 11 | ``` shell 12 | ros2 run popf popf /tmp/${namespace}/domain.pddl /tmp/${namespace}/problem.pddl 13 | ``` 14 | 15 | Use `ros2 run popf popf -h` for more help. 16 | -------------------------------------------------------------------------------- /plansys2_docs/developer_guide.md: -------------------------------------------------------------------------------- 1 | # Developer Guide 2 | 3 | Plansys2 is composed by next modules: 4 | 5 | - [Domain Expert Module](../plansys2_domain_expert/README.md) 6 | - [Problem Expert Module](../plansys2_problem_expert/README.md) 7 | - [Planner](../plansys2_planner/README.md) 8 | - [Executor](../plansys2_executor/README.md) 9 | -------------------------------------------------------------------------------- /plansys2_docs/plansys2_arch.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PlanSys2/ros2_planning_system/b30a2fce581b898a4ed1a2c92e6a7fb8b40b9f14/plansys2_docs/plansys2_arch.png -------------------------------------------------------------------------------- /plansys2_docs/plansys2_logo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PlanSys2/ros2_planning_system/b30a2fce581b898a4ed1a2c92e6a7fb8b40b9f14/plansys2_docs/plansys2_logo.png -------------------------------------------------------------------------------- /plansys2_docs/plansys2_logo_v2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PlanSys2/ros2_planning_system/b30a2fce581b898a4ed1a2c92e6a7fb8b40b9f14/plansys2_docs/plansys2_logo_v2.png -------------------------------------------------------------------------------- /plansys2_docs/plansys2_logo_v2.xcf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PlanSys2/ros2_planning_system/b30a2fce581b898a4ed1a2c92e6a7fb8b40b9f14/plansys2_docs/plansys2_logo_v2.xcf -------------------------------------------------------------------------------- /plansys2_docs/plansys2_logo_v3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PlanSys2/ros2_planning_system/b30a2fce581b898a4ed1a2c92e6a7fb8b40b9f14/plansys2_docs/plansys2_logo_v3.png -------------------------------------------------------------------------------- /plansys2_docs/tutorials.md: -------------------------------------------------------------------------------- 1 | # Tutorials 2 | 3 | Plansys2 is a PDDL-based planning system, so maybe the first reads are PDDL basics [1](https://arxiv.org/pdf/1106.4561.pdf), [2](http://www.cs.toronto.edu/~sheila/2542/w09/A1/introtopddl2.pdf) and [3](http://www.cs.toronto.edu/~sheila/384/w11/Assignments/A3/veloso-PDDL_by_Example.pdf) 4 | 5 | 1. [Working with Plansys2 and Terminal](tutorials/tut_1_terminal.md) 6 | 1. [Patrol navigation Example](tutorials/tut_2_patrol.md) 7 | -------------------------------------------------------------------------------- /plansys2_domain_expert/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | plansys2_domain_expert 5 | 3.0.0 6 | 7 | This package contains the Domain Expert module for the ROS2 Planning System 8 | 9 | Francisco Martin Rico 10 | 11 | Apache License, Version 2.0 12 | 13 | ament_cmake 14 | 15 | rclcpp 16 | rclcpp_lifecycle 17 | ament_index_cpp 18 | plansys2_pddl_parser 19 | plansys2_msgs 20 | plansys2_core 21 | plansys2_popf_plan_solver 22 | std_msgs 23 | 24 | 25 | ament_lint_common 26 | ament_lint_auto 27 | ament_cmake_gtest 28 | lifecycle_msgs 29 | 30 | 31 | ament_cmake 32 | 33 | 34 | -------------------------------------------------------------------------------- /plansys2_domain_expert/src/domain_expert_node.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 | 17 | #include "plansys2_domain_expert/DomainExpertNode.hpp" 18 | #include "rclcpp/rclcpp.hpp" 19 | 20 | int main(int argc, char ** argv) 21 | { 22 | rclcpp::init(argc, argv); 23 | auto node = std::make_shared(); 24 | 25 | rclcpp::experimental::executors::EventsExecutor executor; 26 | executor.add_node(node->get_node_base_interface()); 27 | executor.spin(); 28 | executor.remove_node(node->get_node_base_interface()); 29 | rclcpp::shutdown(); 30 | 31 | return 0; 32 | } 33 | -------------------------------------------------------------------------------- /plansys2_domain_expert/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_domain_expert/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_domain_expert/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 | (:durative-action talk 38 | :parameters (?r - robot ?p - person ?m - message) 39 | :duration ( = ?duration 5) 40 | :condition (and 41 | (over all(robot_near_person ?r ?p)) 42 | ) 43 | :effect (and 44 | (at end(robot_talk ?r ?m ?p)) 45 | ) 46 | ) 47 | 48 | (:durative-action approach 49 | :parameters (?r - robot ?ro - room ?p - person) 50 | :duration ( = ?duration 5) 51 | :condition (and 52 | (over all(robot_at ?r ?ro)) 53 | (over all(person_at ?p ?ro)) 54 | ) 55 | :effect (and 56 | (at end(robot_near_person ?r ?p)) 57 | ) 58 | ) 59 | 60 | 61 | 62 | );; end Domain ;;;;;;;;;;;;;;;;;;;;;;;; 63 | -------------------------------------------------------------------------------- /plansys2_domain_expert/test/pddl/domain_simple_ext.pddl: -------------------------------------------------------------------------------- 1 | (define (domain plansys2) 2 | (:requirements :strips :typing :adl :fluents :durative-actions) 3 | 4 | ;; Types ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; 5 | (:types 6 | robot 7 | pickable_object 8 | room 9 | );; end Types ;;;;;;;;;;;;;;;;;;;;;;;;; 10 | 11 | ;; Predicates ;;;;;;;;;;;;;;;;;;;;;;;;; 12 | (:predicates 13 | 14 | (robot_at ?r - robot ?ro - room) 15 | (object_at_robot ?o - pickable_object ?r - robot) 16 | (object_at_room ?o - pickable_object ?ro - room) 17 | 18 | );; end Predicates ;;;;;;;;;;;;;;;;;;;; 19 | ;; Functions ;;;;;;;;;;;;;;;;;;;;;;;;; 20 | (:functions 21 | 22 | );; end Functions ;;;;;;;;;;;;;;;;;;;; 23 | ;; Actions ;;;;;;;;;;;;;;;;;;;;;;;;;;;; 24 | (:durative-action pick_object 25 | :parameters (?r - robot ?ro - room ?o - pickable_object) 26 | :duration ( = ?duration 5) 27 | :condition (and 28 | (at start(object_at_room ?o ?ro)) 29 | (at start(robot_at ?r ?ro)) 30 | ) 31 | :effect (and 32 | (at start(not(object_at_room ?o ?ro))) 33 | (at end(object_at_robot ?o ?r)) 34 | ) 35 | ) 36 | 37 | (:durative-action place_object 38 | :parameters (?r - robot ?ro - room ?o - pickable_object) 39 | :duration ( = ?duration 5) 40 | :condition (and 41 | (at start(object_at_robot ?o ?r)) 42 | (at start(robot_at ?r ?ro)) 43 | ) 44 | :effect (and 45 | (at start(not(object_at_robot ?o ?r))) 46 | (at end(object_at_room ?o ?ro)) 47 | ) 48 | ) 49 | );; end Domain ;;;;;;;;;;;;;;;;;;;;;;;; 50 | -------------------------------------------------------------------------------- /plansys2_domain_expert/test/pddl/domain_simple_processed.pddl: -------------------------------------------------------------------------------- 1 | (define (domain plansys2) 2 | (:requirements :adl :durative-actions :fluents :strips :typing ) 3 | 4 | (:types 5 | person - object 6 | message - object 7 | robot - object 8 | room - object 9 | teleporter_room - room 10 | ) 11 | 12 | (:predicates 13 | (person_at ?p - person ?ro - room) 14 | (robot_at ?r - robot ?ro - room) 15 | (robot_near_person ?r - robot ?p - person) 16 | (robot_talk ?r - robot ?m - message ?p - person) 17 | ) 18 | 19 | (:functions 20 | (teleportation_time ?from - teleporter_room ?to - room) 21 | ) 22 | 23 | (:durative-action move 24 | :parameters (?r - robot ?r1 ?r2 - room) 25 | :duration ( = ?duration 5) 26 | :condition (and 27 | (at start(robot_at ?r ?r1))) 28 | :effect (and 29 | (at start(not(robot_at ?r ?r1))) 30 | (at end(robot_at ?r ?r2)) 31 | ) 32 | ) 33 | (:durative-action talk 34 | :parameters (?r - robot ?from ?p - person ?m - message) 35 | :duration ( = ?duration 5) 36 | :condition (and 37 | (over all(robot_near_person ?r ?p)) 38 | ) 39 | :effect (and 40 | (at end(robot_talk ?r ?m ?p)) 41 | ) 42 | ) 43 | (:durative-action approach 44 | :parameters (?r - robot ?ro - room ?p - person) 45 | :duration ( = ?duration 5) 46 | :condition (and 47 | (over all(robot_at ?r ?ro)) 48 | (over all(person_at ?p ?ro)) 49 | ) 50 | :effect (and 51 | (at end(robot_near_person ?r ?p)) 52 | ) 53 | ) 54 | (:action move_person 55 | :parameters (?p - person ?r1 ?r2 - room) 56 | :precondition (and 57 | (person_at ?p ?r1) 58 | ) 59 | :effect (and 60 | (person_at ?p ?r2) 61 | (not(person_at ?p ?r1)) 62 | ) 63 | ) 64 | ) 65 | -------------------------------------------------------------------------------- /plansys2_domain_expert/test/pddl/problem_0.pddl: -------------------------------------------------------------------------------- 1 | (define (problem void) 2 | (:domain plansys2) 3 | ) 4 | -------------------------------------------------------------------------------- /plansys2_domain_expert/test/pddl/problem_charging.pddl: -------------------------------------------------------------------------------- 1 | (define (problem charging-problem) 2 | (:domain charging) 3 | 4 | ;; Instantiate the objects. 5 | (:objects 6 | r2d2 - robot 7 | wp_control - waypoint 8 | wp1 wp2 wp3 wp4 - waypoint 9 | ) 10 | 11 | (:init 12 | ; Define the initial state predicates. 13 | (robot_at r2d2 wp_control) 14 | (charger_at wp3) 15 | 16 | (connected wp_control wp1) 17 | (connected wp1 wp_control) 18 | (connected wp_control wp2) 19 | (connected wp2 wp_control) 20 | (connected wp_control wp3) 21 | (connected wp3 wp_control) 22 | (connected wp_control wp4) 23 | (connected wp4 wp_control) 24 | 25 | ; Define static functions 26 | (= (speed r2d2) 3) 27 | (= (max_range r2d2) 75) 28 | (= (state_of_charge r2d2) 99) 29 | 30 | (= (distance wp1 wp2) 30) 31 | (= (distance wp1 wp3) 35) 32 | (= (distance wp1 wp4) 40) 33 | (= (distance wp1 wp_control) 45) 34 | (= (distance wp_control wp1) 45) 35 | (= (distance wp4 wp1) 40) 36 | (= (distance wp3 wp1) 35) 37 | (= (distance wp2 wp1) 30) 38 | 39 | (= (distance wp2 wp3) 45) 40 | (= (distance wp2 wp4) 35) 41 | (= (distance wp2 wp_control) 30) 42 | (= (distance wp_control wp2) 30) 43 | (= (distance wp4 wp2) 35) 44 | (= (distance wp3 wp2) 45) 45 | 46 | (= (distance wp3 wp4) 40) 47 | (= (distance wp3 wp_control) 45) 48 | (= (distance wp_control wp3) 45) 49 | (= (distance wp4 wp3) 40) 50 | 51 | (= (distance wp4 wp_control) 40) 52 | (= (distance wp_control wp4) 40) 53 | ) 54 | 55 | (:goal (and 56 | (patrolled wp1) 57 | (patrolled wp2) 58 | (patrolled wp3) 59 | (patrolled wp4) 60 | )) 61 | 62 | (:metric 63 | minimize (total-time) 64 | ) 65 | ) 66 | -------------------------------------------------------------------------------- /plansys2_domain_expert/test/pddl/problem_simple_1.pddl: -------------------------------------------------------------------------------- 1 | (define (problem plansys2_1) 2 | (:domain plansys2) 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_domain_expert/test/unit/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ament_add_gtest(domain_expert_test domain_expert_test.cpp) 2 | target_link_libraries(domain_expert_test 3 | ${PROJECT_NAME} 4 | ament_index_cpp::ament_index_cpp 5 | ${lifecycle_msgs_TARGETS} 6 | ) 7 | 8 | ament_add_gtest(domain_expert_node_test domain_expert_node_test.cpp) 9 | target_link_libraries(domain_expert_node_test 10 | ${PROJECT_NAME} 11 | ament_index_cpp::ament_index_cpp 12 | ) 13 | 14 | ament_add_gtest(domain_reader_test domain_reader_test.cpp) 15 | target_link_libraries(domain_reader_test 16 | ${PROJECT_NAME} 17 | ament_index_cpp::ament_index_cpp 18 | ) 19 | 20 | ament_add_gtest(types_test types_test.cpp) 21 | target_link_libraries(types_test 22 | ${PROJECT_NAME} 23 | ament_index_cpp::ament_index_cpp 24 | ) -------------------------------------------------------------------------------- /plansys2_executor/behavior_trees/plansys2_action_bt.xml: -------------------------------------------------------------------------------- 1 | 2 | WAIT_PREV_ACTIONS 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /plansys2_executor/behavior_trees/plansys2_action_bt_with_undo.xml: -------------------------------------------------------------------------------- 1 | 2 | WAIT_PREV_ACTIONS 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /plansys2_executor/behavior_trees/plansys2_end_action_bt.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | CHECK_PREV_ACTIONS 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /plansys2_executor/behavior_trees/plansys2_start_action_bt.xml: -------------------------------------------------------------------------------- 1 | 2 | WAIT_PREV_ACTIONS 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /plansys2_executor/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | plansys2_executor 5 | 3.0.0 6 | 7 | This package contains the Executor module for the ROS2 Planning System 8 | 9 | Francisco Martin Rico 10 | 11 | Apache License, Version 2.0 12 | 13 | ament_cmake 14 | eigen3_cmake_module 15 | 16 | eigen 17 | 18 | rclcpp 19 | rclcpp_action 20 | rclcpp_lifecycle 21 | lifecycle_msgs 22 | rclcpp_cascade_lifecycle 23 | ament_index_cpp 24 | plansys2_core 25 | plansys2_pddl_parser 26 | plansys2_msgs 27 | plansys2_domain_expert 28 | plansys2_problem_expert 29 | plansys2_planner 30 | pluginlib 31 | behaviortree_cpp 32 | std_msgs 33 | std_srvs 34 | 35 | popf 36 | 37 | ament_lint_common 38 | ament_lint_auto 39 | ament_cmake_gtest 40 | 41 | 42 | ament_cmake 43 | 44 | 45 | 46 | -------------------------------------------------------------------------------- /plansys2_executor/plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /plansys2_executor/src/compute_bt.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 | 17 | #include "plansys2_executor/ComputeBT.hpp" 18 | #include "rclcpp/rclcpp.hpp" 19 | 20 | int main(int argc, char ** argv) 21 | { 22 | rclcpp::init(argc, argv); 23 | auto node = std::make_shared(); 24 | 25 | node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); 26 | std::this_thread::sleep_for(std::chrono::milliseconds(500)); 27 | node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE); 28 | std::this_thread::sleep_for(std::chrono::milliseconds(500)); 29 | 30 | rclcpp::spin(node->get_node_base_interface()); 31 | 32 | rclcpp::shutdown(); 33 | 34 | return 0; 35 | } 36 | -------------------------------------------------------------------------------- /plansys2_executor/src/executor_node.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 | 17 | #include "plansys2_executor/ExecutorNode.hpp" 18 | #include "rclcpp/rclcpp.hpp" 19 | 20 | int main(int argc, char ** argv) 21 | { 22 | rclcpp::init(argc, argv); 23 | auto node = std::make_shared(); 24 | 25 | rclcpp::spin(node->get_node_base_interface()); 26 | 27 | rclcpp::shutdown(); 28 | 29 | return 0; 30 | } 31 | -------------------------------------------------------------------------------- /plansys2_executor/src/plansys2_executor/behavior_tree/apply_atend_effect_node.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2020 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 | 19 | #include "plansys2_executor/behavior_tree/apply_atend_effect_node.hpp" 20 | #include "plansys2_msgs/msg/tree.hpp" 21 | 22 | namespace plansys2 23 | { 24 | 25 | ApplyAtEndEffect::ApplyAtEndEffect( 26 | const std::string & xml_tag_name, 27 | const BT::NodeConfig & conf) 28 | : ActionNodeBase(xml_tag_name, conf) 29 | { 30 | action_map_ = 31 | config().blackboard->get>>( 32 | "action_map"); 33 | 34 | problem_client_ = 35 | config().blackboard->get>( 36 | "problem_client"); 37 | } 38 | 39 | BT::NodeStatus 40 | ApplyAtEndEffect::tick() 41 | { 42 | std::string action; 43 | getInput("action", action); 44 | 45 | auto effect = (*action_map_)[action].action_info.get_at_end_effects(); 46 | 47 | if (!(*action_map_)[action].at_end_effects_applied) { 48 | (*action_map_)[action].at_end_effects_applied = true; 49 | apply(effect, problem_client_, 0); 50 | } 51 | 52 | return BT::NodeStatus::SUCCESS; 53 | } 54 | 55 | } // namespace plansys2 56 | -------------------------------------------------------------------------------- /plansys2_executor/src/plansys2_executor/behavior_tree/apply_atstart_effect_node.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2020 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 | 19 | #include "plansys2_executor/behavior_tree/apply_atstart_effect_node.hpp" 20 | 21 | namespace plansys2 22 | { 23 | 24 | ApplyAtStartEffect::ApplyAtStartEffect( 25 | const std::string & xml_tag_name, 26 | const BT::NodeConfig & conf) 27 | : ActionNodeBase(xml_tag_name, conf) 28 | { 29 | action_map_ = 30 | config().blackboard->get>>( 31 | "action_map"); 32 | 33 | problem_client_ = 34 | config().blackboard->get>( 35 | "problem_client"); 36 | } 37 | 38 | BT::NodeStatus 39 | ApplyAtStartEffect::tick() 40 | { 41 | std::string action; 42 | getInput("action", action); 43 | 44 | if ((*action_map_)[action].action_info.is_action()) { 45 | return BT::NodeStatus::SUCCESS; 46 | } 47 | 48 | auto effect = (*action_map_)[action].action_info.get_at_start_effects(); 49 | 50 | if (!(*action_map_)[action].at_start_effects_applied) { 51 | (*action_map_)[action].at_start_effects_applied = true; 52 | apply(effect, problem_client_, 0); 53 | } 54 | 55 | return BT::NodeStatus::SUCCESS; 56 | } 57 | 58 | } // namespace plansys2 59 | -------------------------------------------------------------------------------- /plansys2_executor/src/plansys2_executor/behavior_tree/wait_action_node.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2020 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 | 19 | #include "plansys2_executor/behavior_tree/wait_action_node.hpp" 20 | 21 | namespace plansys2 22 | { 23 | 24 | WaitAction::WaitAction( 25 | const std::string & xml_tag_name, 26 | const BT::NodeConfig & conf) 27 | : ActionNodeBase(xml_tag_name, conf) 28 | { 29 | action_map_ = 30 | config().blackboard->get>>( 31 | "action_map"); 32 | } 33 | 34 | BT::NodeStatus 35 | WaitAction::tick() 36 | { 37 | std::string action; 38 | getInput("action", action); 39 | 40 | if ((*action_map_).find(action) == (*action_map_).end()) { 41 | return BT::NodeStatus::RUNNING; // Not started yet 42 | } 43 | 44 | if ((*action_map_)[action].action_executor != nullptr && 45 | (*action_map_)[action].action_executor->is_finished()) 46 | { 47 | return BT::NodeStatus::SUCCESS; 48 | } else { 49 | return BT::NodeStatus::RUNNING; 50 | } 51 | } 52 | 53 | } // namespace plansys2 54 | -------------------------------------------------------------------------------- /plansys2_executor/test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(TEST_PDDL_DIR ${CMAKE_CURRENT_SOURCE_DIR}/pddl) 2 | set(TEST_BEHAVIOR_TREES_DIR ${CMAKE_CURRENT_SOURCE_DIR}/test_behavior_trees) 3 | set(TEST_DATA_DIR ${CMAKE_CURRENT_SOURCE_DIR}/test_data) 4 | set(TEST_LAUNCH_DIR ${CMAKE_CURRENT_SOURCE_DIR}/test_launch_files) 5 | 6 | install(DIRECTORY 7 | ${TEST_PDDL_DIR} 8 | ${TEST_BEHAVIOR_TREES_DIR} 9 | ${TEST_DATA_DIR} 10 | DESTINATION share/${PROJECT_NAME} 11 | ) 12 | 13 | add_subdirectory(unit) 14 | #add_subdirectory(integration) 15 | -------------------------------------------------------------------------------- /plansys2_executor/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 | (: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 | :effect (and 32 | (at start(not(robot_at ?r ?r1))) 33 | (at end(robot_at ?r ?r2)) 34 | ) 35 | ) 36 | 37 | (:durative-action talk 38 | :parameters (?r - robot ?from ?p - person ?m - message) 39 | :duration ( = ?duration 5) 40 | :condition (and 41 | (over all(robot_near_person ?r ?p)) 42 | ) 43 | :effect (and 44 | (at end(robot_talk ?r ?m ?p)) 45 | ) 46 | ) 47 | 48 | (:durative-action approach 49 | :parameters (?r - robot ?ro - room ?p - person) 50 | :duration ( = ?duration 5) 51 | :condition (and 52 | (over all(robot_at ?r ?ro)) 53 | (over all(person_at ?p ?ro)) 54 | ) 55 | :effect (and 56 | (at end(robot_near_person ?r ?p)) 57 | ) 58 | ) 59 | 60 | 61 | 62 | );; end Domain ;;;;;;;;;;;;;;;;;;;;;;;; 63 | -------------------------------------------------------------------------------- /plansys2_executor/test/pddl/elevator_problem.pddl: -------------------------------------------------------------------------------- 1 | (define (problem elevators-problem) 2 | (:domain temporal-elevators) 3 | 4 | ;; Instantiate the objects. 5 | (:objects 6 | n1 n2 n3 n4 n5 - num 7 | p1 p2 p3 - passenger 8 | e1 e2 - elevator 9 | ) 10 | 11 | (:init 12 | ; Same fluents as the classical planning example 13 | (next n1 n2) 14 | (next n2 n3) 15 | (next n3 n4) 16 | (next n4 n5) 17 | 18 | (lift_at e1 n1) 19 | (lift_at e2 n5) 20 | 21 | (passenger_at p1 n2) 22 | (passenger_at p2 n2) 23 | (passenger_at p3 n4) 24 | 25 | ; Define how fast each of the passengers move (in seconds) 26 | (= (person_speed p1) 2) 27 | (= (person_speed p2) 3) 28 | (= (person_speed p3) 2) 29 | 30 | ; Define the speed of the elevators (in meters / second) 31 | (= (elevator_speed e1) 2) 32 | (= (elevator_speed e2) 3) 33 | 34 | ; Define the distance between the floors (in meters) 35 | (= (floor_distance n1 n2) 3) 36 | (= (floor_distance n2 n3) 4) 37 | (= (floor_distance n3 n4) 4) 38 | (= (floor_distance n4 n5) 3) 39 | (= (floor_distance n5 n4) 3) 40 | (= (floor_distance n4 n3) 4) 41 | (= (floor_distance n3 n2) 4) 42 | (= (floor_distance n2 n1) 3) 43 | ) 44 | 45 | (:goal (and 46 | (passenger_at p1 n1) 47 | (passenger_at p2 n1) 48 | (passenger_at p3 n1) 49 | )) 50 | 51 | (:metric 52 | minimize (total-time) 53 | ) 54 | ) 55 | -------------------------------------------------------------------------------- /plansys2_executor/test/pddl/problem_factory_1.pddl: -------------------------------------------------------------------------------- 1 | ( define ( problem problem_1 ) 2 | ( :domain factory ) 3 | ( :objects 4 | robot1 robot2 robot3 - robot 5 | wheels_zone steering_wheels_zone body_car_zone assembly_zone - zone 6 | wheel_1 body_car_1 steering_wheel_1 wheel_2 body_car_2 steering_wheel_2 wheel_3 body_car_3 steering_wheel_3 - piece 7 | car_1 car_2 car_3 - car 8 | ) 9 | ( :init 10 | ( robot_at robot1 assembly_zone ) 11 | ( robot_at robot2 assembly_zone ) 12 | ( robot_at robot3 assembly_zone ) 13 | ( is_assembly_zone assembly_zone ) 14 | ( robot_available robot1 ) 15 | ( robot_available robot2 ) 16 | ( robot_available robot3 ) 17 | ( piece_at wheel_1 wheels_zone ) 18 | ( piece_at body_car_1 body_car_zone ) 19 | ( piece_at steering_wheel_1 steering_wheels_zone ) 20 | ( piece_is_wheel wheel_1 ) 21 | ( piece_is_body_car body_car_1 ) 22 | ( piece_is_steering_wheel steering_wheel_1 ) 23 | ( piece_at wheel_2 wheels_zone ) 24 | ( piece_at body_car_2 body_car_zone ) 25 | ( piece_at steering_wheel_2 steering_wheels_zone ) 26 | ( piece_is_wheel wheel_2 ) 27 | ( piece_is_body_car body_car_2 ) 28 | ( piece_is_steering_wheel steering_wheel_2 ) 29 | ( piece_at wheel_3 wheels_zone ) 30 | ( piece_at body_car_3 body_car_zone ) 31 | ( piece_at steering_wheel_3 steering_wheels_zone ) 32 | ( piece_is_wheel wheel_3 ) 33 | ( piece_is_body_car body_car_3 ) 34 | ( piece_is_steering_wheel steering_wheel_3 ) 35 | ( piece_not_used wheel_1 ) 36 | ( piece_not_used wheel_2 ) 37 | ( piece_not_used wheel_3 ) 38 | ( piece_not_used body_car_1 ) 39 | ( piece_not_used body_car_2 ) 40 | ( piece_not_used body_car_3 ) 41 | ( piece_not_used steering_wheel_1 ) 42 | ( piece_not_used steering_wheel_2 ) 43 | ( piece_not_used steering_wheel_3 ) 44 | ) 45 | ( :goal 46 | ( and 47 | ( car_assembled car_1 ) 48 | ( car_assembled car_2 ) 49 | ( car_assembled car_3 ) 50 | ) 51 | ) 52 | ) 53 | -------------------------------------------------------------------------------- /plansys2_executor/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_executor/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_executor/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_executor/test/pddl/simple_move_example.pddl: -------------------------------------------------------------------------------- 1 | (define (domain replan) 2 | (:requirements :strips :typing :adl :fluents :durative-actions) 3 | 4 | ;; Types ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; 5 | (:types 6 | robot 7 | waypoint 8 | );; end Types ;;;;;;;;;;;;;;;;;;;;;;;;; 9 | 10 | ;; Predicates ;;;;;;;;;;;;;;;;;;;;;;;;; 11 | (:predicates 12 | 13 | 14 | (robot_at ?r - robot ?wp - waypoint) 15 | (connected ?wp_from ?wp_to - waypoint) 16 | 17 | 18 | );; end Predicates ;;;;;;;;;;;;;;;;;;;; 19 | ;; Functions ;;;;;;;;;;;;;;;;;;;;;;;;; 20 | (:functions 21 | 22 | );; end Functions ;;;;;;;;;;;;;;;;;;;; 23 | ;; Actions ;;;;;;;;;;;;;;;;;;;;;;;;;;;; 24 | (:durative-action move 25 | :parameters (?r - robot ?from ?to - waypoint) 26 | :duration ( = ?duration 5) 27 | :condition (and 28 | (at start(robot_at ?r ?from)) 29 | (over all(connected ?from ?to)) 30 | ) 31 | :effect (and 32 | (at end(not(robot_at ?r ?from))) 33 | (at end(robot_at ?r ?to)) 34 | ) 35 | ) 36 | 37 | );; end Domain ;;;;;;;;;;;;;;;;;;;;;;;; 38 | -------------------------------------------------------------------------------- /plansys2_executor/test/test_behavior_trees/test_action_timeout_bt.xml: -------------------------------------------------------------------------------- 1 | 2 | WAIT_PREV_ACTIONS 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /plansys2_executor/test/test_behavior_trees/test_action_timeout_end_bt.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | CHECK_PREV_ACTIONS 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /plansys2_executor/test/test_behavior_trees/test_action_timeout_start_bt.xml: -------------------------------------------------------------------------------- 1 | 2 | WAIT_PREV_ACTIONS 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /plansys2_executor/test/test_data/elevator_graph.csv: -------------------------------------------------------------------------------- 1 | 0, 0, 0, move-down e2 n5 n4, board p3 n4 e2 2 | 0, 2, 1, board p3 n4 e2, move-down e2 n4 n3 3 | 0, 5, 3, move-down e2 n4 n3, move-down e2 n3 n2 4 | 0, 6, 4, move-down e2 n3 n2, move-down e2 n2 n1 5 | 0, 8, 6, move-down e2 n2 n1, leave p3 n1 e2 6 | 0, 11, 8, leave p3 n1 e2 7 | 1, 1, 0, move-up e1 n1 n2, board p1 n2 e1, board p2 n2 e1 8 | 1, 3, 2, board p1 n2 e1, move-down e1 n2 n1 9 | 1, 7, 5, move-down e1 n2 n1, leave p1 n1 e1, leave p2 n1 e1 10 | 1, 9, 7, leave p1 n1 e1 11 | 1, 10, 7, leave p2 n1 e1 12 | 1, 4, 2, board p2 n2 e1, move-down e1 n2 n1 13 | 1, 7, 5, move-down e1 n2 n1, leave p1 n1 e1, leave p2 n1 e1 14 | 1, 9, 7, leave p1 n1 e1 15 | 1, 10, 7, leave p2 n1 e1 16 | -------------------------------------------------------------------------------- /plansys2_executor/test/test_data/road_trip_graph.csv: -------------------------------------------------------------------------------- 1 | 0, 0, 0, move herbie wp0 wp1, visit herbie wp1 2 | 0, 1, 1, visit herbie wp1, move herbie wp1 rp0 3 | 0, 2, 2, move herbie wp1 rp0, refuel herbie rp0 4 | 0, 3, 3, refuel herbie rp0, move herbie rp0 wp3 5 | 0, 4, 4, move herbie rp0 wp3, visit herbie wp3 6 | 0, 5, 5, visit herbie wp3, move herbie wp3 wp2 7 | 0, 6, 6, move herbie wp3 wp2, visit herbie wp2 8 | 0, 7, 7, visit herbie wp2 9 | -------------------------------------------------------------------------------- /plansys2_executor/test/unit/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ament_add_gtest(executor_test executor_test.cpp TIMEOUT 300) 2 | target_compile_definitions(executor_test 3 | PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") 4 | target_link_libraries(executor_test 5 | ${PROJECT_NAME} 6 | ) 7 | 8 | ament_add_gtest(action_execution_test action_execution_test.cpp) 9 | target_compile_definitions(action_execution_test 10 | PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") 11 | target_link_libraries(action_execution_test 12 | ${PROJECT_NAME} 13 | ) 14 | 15 | ament_add_gtest(execution_tree_test execution_tree_test.cpp) 16 | target_compile_definitions(execution_tree_test 17 | PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") 18 | target_link_libraries(execution_tree_test 19 | ${PROJECT_NAME} 20 | ) 21 | 22 | ament_add_gtest(simple_btbuilder_tests simple_btbuilder_tests.cpp) 23 | target_compile_definitions(simple_btbuilder_tests 24 | PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") 25 | target_link_libraries(simple_btbuilder_tests 26 | ${PROJECT_NAME} 27 | bt_builder_plugins 28 | ) 29 | 30 | ament_add_gtest(sequential_bt_builder_test sequential_bt_builder_test.cpp) 31 | target_compile_definitions(sequential_bt_builder_test 32 | PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") 33 | target_link_libraries(sequential_bt_builder_test 34 | ${PROJECT_NAME} 35 | bt_builder_plugins 36 | ) 37 | 38 | ament_add_gtest(bt_node_test bt_node_test.cpp) 39 | target_compile_definitions(bt_node_test 40 | PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") 41 | target_link_libraries(bt_node_test 42 | ${PROJECT_NAME} 43 | ) 44 | 45 | ament_add_gtest(bt_node_test_charging bt_node_test_charging.cpp) 46 | target_compile_definitions(bt_node_test_charging 47 | PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") 48 | target_link_libraries(bt_node_test_charging 49 | ${PROJECT_NAME} 50 | ) 51 | -------------------------------------------------------------------------------- /plansys2_lifecycle_manager/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | project(plansys2_lifecycle_manager) 4 | 5 | find_package(ament_cmake REQUIRED) 6 | find_package(rclcpp REQUIRED) 7 | find_package(lifecycle_msgs REQUIRED) 8 | 9 | add_library(${PROJECT_NAME} 10 | src/plansys2_lifecycle_manager/lifecycle_manager.cpp 11 | ) 12 | target_include_directories(${PROJECT_NAME} PUBLIC 13 | "$" 14 | "$" 15 | "$") 16 | target_link_libraries(${PROJECT_NAME} 17 | PUBLIC 18 | rclcpp::rclcpp 19 | ${lifecycle_msgs_TARGETS} 20 | ) 21 | 22 | add_executable(lifecycle_manager_node 23 | src/lifecycle_manager_node.cpp) 24 | target_include_directories(lifecycle_manager_node PRIVATE 25 | "$" 26 | "$" 27 | ) 28 | target_link_libraries(lifecycle_manager_node 29 | PRIVATE 30 | ${PROJECT_NAME} 31 | rclcpp::rclcpp 32 | ) 33 | 34 | install(DIRECTORY include/ 35 | DESTINATION include/${PROJECT_NAME} 36 | ) 37 | 38 | install(TARGETS ${PROJECT_NAME} 39 | EXPORT export_${PROJECT_NAME} 40 | ARCHIVE DESTINATION lib 41 | LIBRARY DESTINATION lib 42 | RUNTIME DESTINATION bin 43 | ) 44 | 45 | install(TARGETS lifecycle_manager_node 46 | RUNTIME DESTINATION lib/${PROJECT_NAME} 47 | ) 48 | 49 | if(BUILD_TESTING) 50 | find_package(ament_lint_auto REQUIRED) 51 | ament_lint_auto_find_test_dependencies() 52 | 53 | find_package(ament_cmake_gtest REQUIRED) 54 | find_package(rclcpp_lifecycle REQUIRED) 55 | add_subdirectory(test) 56 | endif() 57 | 58 | ament_export_include_directories("include/${PROJECT_NAME}") 59 | ament_export_libraries(${PROJECT_NAME}) 60 | ament_export_targets(export_${PROJECT_NAME}) 61 | ament_export_dependencies( 62 | rclcpp 63 | lifecycle_msgs 64 | ) 65 | 66 | ament_package() -------------------------------------------------------------------------------- /plansys2_lifecycle_manager/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | plansys2_lifecycle_manager 5 | 3.0.0 6 | 7 | A controller/manager for the lifecycle nodes of the ROS2 Planning System 8 | 9 | Francisco Martin Rico 10 | 11 | Apache License, Version 2.0 12 | 13 | ament_cmake 14 | 15 | rclcpp 16 | lifecycle_msgs 17 | 18 | ament_lint_common 19 | ament_lint_auto 20 | ament_cmake_gtest 21 | 22 | 23 | ament_cmake 24 | 25 | 26 | -------------------------------------------------------------------------------- /plansys2_lifecycle_manager/test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ament_add_gtest(lf_manager_test lf_manager_test.cpp) 2 | target_link_libraries(lf_manager_test 3 | ${PROJECT_NAME} 4 | rclcpp_lifecycle::rclcpp_lifecycle 5 | ) 6 | -------------------------------------------------------------------------------- /plansys2_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | project(plansys2_msgs) 4 | 5 | find_package(ament_cmake REQUIRED) 6 | find_package(builtin_interfaces REQUIRED) 7 | find_package(rosidl_default_generators REQUIRED) 8 | find_package(std_msgs REQUIRED) 9 | find_package(action_msgs REQUIRED) 10 | 11 | rosidl_generate_interfaces(${PROJECT_NAME} 12 | "msg/Action.msg" 13 | "msg/ActionExecution.msg" 14 | "msg/ActionExecutionInfo.msg" 15 | "msg/ActionPerformerStatus.msg" 16 | "msg/Derived.msg" 17 | "msg/DurativeAction.msg" 18 | "msg/Knowledge.msg" 19 | "msg/Node.msg" 20 | "msg/Param.msg" 21 | "msg/Plan.msg" 22 | "msg/PlanArray.msg" 23 | "msg/PlanItem.msg" 24 | "msg/Problem.msg" 25 | "msg/State.msg" 26 | "msg/Tree.msg" 27 | "srv/AddProblem.srv" 28 | "srv/AddProblemGoal.srv" 29 | "srv/AffectNode.srv" 30 | "srv/AffectParam.srv" 31 | "srv/ExistNode.srv" 32 | "srv/GetDomain.srv" 33 | "srv/GetDomainActions.srv" 34 | "srv/GetDomainActionDetails.srv" 35 | "srv/GetDomainDerivedPredicateDetails.srv" 36 | "srv/GetDomainDurativeActionDetails.srv" 37 | "srv/GetDomainName.srv" 38 | "srv/GetDomainTypes.srv" 39 | "srv/GetDomainConstants.srv" 40 | "srv/GetNodeDetails.srv" 41 | "srv/GetPlan.srv" 42 | "srv/GetPlanArray.srv" 43 | "srv/GetOrderedSubGoals.srv" 44 | "srv/GetProblem.srv" 45 | "srv/GetProblemGoal.srv" 46 | "srv/GetProblemInstances.srv" 47 | "srv/GetProblemInstanceDetails.srv" 48 | "srv/GetStates.srv" 49 | "srv/IsProblemGoalSatisfied.srv" 50 | "srv/RemoveProblemGoal.srv" 51 | "srv/ClearProblemKnowledge.srv" 52 | "srv/ValidateDomain.srv" 53 | "action/ExecutePlan.action" 54 | DEPENDENCIES builtin_interfaces std_msgs action_msgs 55 | ) 56 | 57 | ament_export_dependencies(rosidl_default_runtime) 58 | 59 | ament_package() 60 | -------------------------------------------------------------------------------- /plansys2_msgs/action/ExecutePlan.action: -------------------------------------------------------------------------------- 1 | plansys2_msgs/Plan plan 2 | --- 3 | int8 SUCCESS=1 4 | int8 FAILURE=2 5 | int8 PREEMPT=3 6 | int8 result 7 | plansys2_msgs/ActionExecutionInfo[] action_execution_status 8 | --- 9 | plansys2_msgs/ActionExecutionInfo[] action_execution_status 10 | -------------------------------------------------------------------------------- /plansys2_msgs/msg/Action.msg: -------------------------------------------------------------------------------- 1 | string name 2 | plansys2_msgs/Param[] parameters 3 | 4 | plansys2_msgs/Tree preconditions 5 | plansys2_msgs/Tree effects 6 | -------------------------------------------------------------------------------- /plansys2_msgs/msg/ActionExecution.msg: -------------------------------------------------------------------------------- 1 | int16 REQUEST=1 2 | int16 RESPONSE=2 3 | int16 CONFIRM=3 4 | int16 REJECT=4 5 | int16 FEEDBACK=5 6 | int16 FINISH=6 7 | int16 CANCEL=7 8 | 9 | int16 type 10 | string node_id 11 | 12 | string action 13 | string[] arguments 14 | 15 | bool success 16 | float32 completion 17 | string status 18 | -------------------------------------------------------------------------------- /plansys2_msgs/msg/ActionExecutionInfo.msg: -------------------------------------------------------------------------------- 1 | int8 NOT_EXECUTED=1 2 | int8 EXECUTING=2 3 | int8 FAILED=3 4 | int8 SUCCEEDED=4 5 | int8 CANCELLED=5 6 | 7 | int8 status 8 | 9 | builtin_interfaces/Time start_stamp 10 | builtin_interfaces/Time status_stamp 11 | 12 | string action_full_name 13 | 14 | string action 15 | string[] arguments 16 | 17 | # predicted duration of the action from the planner 18 | builtin_interfaces/Duration duration 19 | 20 | float32 completion 21 | string message_status 22 | -------------------------------------------------------------------------------- /plansys2_msgs/msg/ActionPerformerStatus.msg: -------------------------------------------------------------------------------- 1 | int8 NOT_READY=0 2 | int8 READY=1 3 | int8 RUNNING=2 4 | int8 FAILURE=4 5 | 6 | builtin_interfaces/Time status_stamp 7 | 8 | int8 state 9 | string action 10 | string[] specialized_arguments 11 | string node_name 12 | -------------------------------------------------------------------------------- /plansys2_msgs/msg/Derived.msg: -------------------------------------------------------------------------------- 1 | plansys2_msgs/Node predicate 2 | plansys2_msgs/Tree preconditions 3 | -------------------------------------------------------------------------------- /plansys2_msgs/msg/DurativeAction.msg: -------------------------------------------------------------------------------- 1 | string name 2 | plansys2_msgs/Param[] parameters 3 | 4 | plansys2_msgs/Tree at_start_requirements 5 | plansys2_msgs/Tree over_all_requirements 6 | plansys2_msgs/Tree at_end_requirements 7 | plansys2_msgs/Tree at_start_effects 8 | plansys2_msgs/Tree at_end_effects 9 | -------------------------------------------------------------------------------- /plansys2_msgs/msg/Knowledge.msg: -------------------------------------------------------------------------------- 1 | string[] instances 2 | string[] predicates 3 | string[] functions 4 | string goal 5 | -------------------------------------------------------------------------------- /plansys2_msgs/msg/Node.msg: -------------------------------------------------------------------------------- 1 | # Unknown type 2 | uint8 UNKNOWN = 0 3 | 4 | # Node types 5 | uint8 AND = 1 6 | uint8 OR = 2 7 | uint8 NOT = 3 8 | uint8 ACTION = 4 9 | uint8 PREDICATE = 5 10 | uint8 FUNCTION = 6 11 | uint8 EXPRESSION = 7 12 | uint8 FUNCTION_MODIFIER = 8 13 | uint8 NUMBER = 9 14 | uint8 CONSTANT = 10 15 | uint8 PARAMETER = 11 16 | uint8 EXISTS = 12 17 | 18 | # Expression types 19 | uint8 COMP_EQ = 13 20 | uint8 COMP_GE = 14 21 | uint8 COMP_GT = 15 22 | uint8 COMP_LE = 16 23 | uint8 COMP_LT = 17 24 | uint8 ARITH_MULT = 18 25 | uint8 ARITH_DIV = 19 26 | uint8 ARITH_ADD = 20 27 | uint8 ARITH_SUB = 21 28 | 29 | # Function modifier types 30 | uint8 ASSIGN = 22 31 | uint8 INCREASE = 23 32 | uint8 DECREASE = 24 33 | uint8 SCALE_UP = 25 34 | uint8 SCALE_DOWN = 26 35 | 36 | uint8 node_type 37 | uint8 expression_type 38 | uint8 modifier_type 39 | 40 | uint32 node_id 41 | uint32[] children 42 | 43 | string name 44 | plansys2_msgs/Param[] parameters 45 | float64 value 46 | bool negate 47 | -------------------------------------------------------------------------------- /plansys2_msgs/msg/Param.msg: -------------------------------------------------------------------------------- 1 | string name 2 | string type 3 | string[] sub_types 4 | -------------------------------------------------------------------------------- /plansys2_msgs/msg/Plan.msg: -------------------------------------------------------------------------------- 1 | plansys2_msgs/PlanItem[] items 2 | -------------------------------------------------------------------------------- /plansys2_msgs/msg/PlanArray.msg: -------------------------------------------------------------------------------- 1 | plansys2_msgs/Plan[] plan_array 2 | -------------------------------------------------------------------------------- /plansys2_msgs/msg/PlanItem.msg: -------------------------------------------------------------------------------- 1 | float32 time 2 | string action 3 | float32 duration 4 | -------------------------------------------------------------------------------- /plansys2_msgs/msg/Problem.msg: -------------------------------------------------------------------------------- 1 | builtin_interfaces/Time stamp 2 | string problem 3 | -------------------------------------------------------------------------------- /plansys2_msgs/msg/State.msg: -------------------------------------------------------------------------------- 1 | plansys2_msgs/Param[] instances 2 | plansys2_msgs/Node[] predicates 3 | plansys2_msgs/Node[] inferred_predicates 4 | plansys2_msgs/Node[] functions 5 | 6 | plansys2_msgs/Derived[] derived_predicates 7 | -------------------------------------------------------------------------------- /plansys2_msgs/msg/Tree.msg: -------------------------------------------------------------------------------- 1 | # A tree used to represent a PDDL construct in PlanSys2 2 | 3 | plansys2_msgs/Node[] nodes 4 | -------------------------------------------------------------------------------- /plansys2_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | plansys2_msgs 5 | 3.0.0 6 | 7 | Messages and service files for the ROS2 Planning System 8 | 9 | Francisco Martin Rico 10 | 11 | Apache License, Version 2.0 12 | 13 | ament_cmake 14 | 15 | rclcpp 16 | std_msgs 17 | action_msgs 18 | builtin_interfaces 19 | rosidl_default_generators 20 | 21 | rosidl_interface_packages 22 | 23 | 24 | ament_cmake 25 | 26 | 27 | -------------------------------------------------------------------------------- /plansys2_msgs/srv/AddProblem.srv: -------------------------------------------------------------------------------- 1 | string problem 2 | --- 3 | bool success 4 | string error_info 5 | -------------------------------------------------------------------------------- /plansys2_msgs/srv/AddProblemGoal.srv: -------------------------------------------------------------------------------- 1 | plansys2_msgs/Tree tree 2 | --- 3 | bool success 4 | string error_info 5 | -------------------------------------------------------------------------------- /plansys2_msgs/srv/AffectNode.srv: -------------------------------------------------------------------------------- 1 | plansys2_msgs/Node node 2 | --- 3 | bool success 4 | string error_info 5 | -------------------------------------------------------------------------------- /plansys2_msgs/srv/AffectParam.srv: -------------------------------------------------------------------------------- 1 | plansys2_msgs/Param param 2 | --- 3 | bool success 4 | string error_info 5 | -------------------------------------------------------------------------------- /plansys2_msgs/srv/ClearProblemKnowledge.srv: -------------------------------------------------------------------------------- 1 | std_msgs/Empty request 2 | --- 3 | bool success 4 | string error_info -------------------------------------------------------------------------------- /plansys2_msgs/srv/ExistNode.srv: -------------------------------------------------------------------------------- 1 | plansys2_msgs/Node node 2 | --- 3 | bool exist 4 | -------------------------------------------------------------------------------- /plansys2_msgs/srv/GetDomain.srv: -------------------------------------------------------------------------------- 1 | std_msgs/Empty request 2 | --- 3 | bool success 4 | string domain 5 | string error_info 6 | -------------------------------------------------------------------------------- /plansys2_msgs/srv/GetDomainActionDetails.srv: -------------------------------------------------------------------------------- 1 | string action 2 | string[] parameters 3 | --- 4 | plansys2_msgs/Action action 5 | bool success 6 | string error_info 7 | -------------------------------------------------------------------------------- /plansys2_msgs/srv/GetDomainActions.srv: -------------------------------------------------------------------------------- 1 | std_msgs/Empty request 2 | --- 3 | bool success 4 | string[] actions 5 | string error_info 6 | -------------------------------------------------------------------------------- /plansys2_msgs/srv/GetDomainConstants.srv: -------------------------------------------------------------------------------- 1 | string type 2 | --- 3 | bool success 4 | string[] constants 5 | string error_info 6 | -------------------------------------------------------------------------------- /plansys2_msgs/srv/GetDomainDerivedPredicateDetails.srv: -------------------------------------------------------------------------------- 1 | string predicate 2 | --- 3 | plansys2_msgs/Derived[] predicates 4 | bool success 5 | string error_info 6 | -------------------------------------------------------------------------------- /plansys2_msgs/srv/GetDomainDurativeActionDetails.srv: -------------------------------------------------------------------------------- 1 | string durative_action 2 | string[] parameters 3 | --- 4 | bool success 5 | plansys2_msgs/DurativeAction durative_action 6 | string error_info 7 | -------------------------------------------------------------------------------- /plansys2_msgs/srv/GetDomainName.srv: -------------------------------------------------------------------------------- 1 | std_msgs/Empty request 2 | --- 3 | bool success 4 | string name 5 | string error_info 6 | -------------------------------------------------------------------------------- /plansys2_msgs/srv/GetDomainTypes.srv: -------------------------------------------------------------------------------- 1 | std_msgs/Empty request 2 | --- 3 | bool success 4 | string[] types 5 | string error_info 6 | -------------------------------------------------------------------------------- /plansys2_msgs/srv/GetNodeDetails.srv: -------------------------------------------------------------------------------- 1 | string expression 2 | --- 3 | bool success 4 | plansys2_msgs/Node node 5 | string error_info 6 | -------------------------------------------------------------------------------- /plansys2_msgs/srv/GetOrderedSubGoals.srv: -------------------------------------------------------------------------------- 1 | std_msgs/Empty request 2 | --- 3 | bool success 4 | plansys2_msgs/Tree[] sub_goals 5 | string error_info 6 | -------------------------------------------------------------------------------- /plansys2_msgs/srv/GetPlan.srv: -------------------------------------------------------------------------------- 1 | string domain 2 | string problem 3 | --- 4 | bool success 5 | plansys2_msgs/Plan plan 6 | string error_info 7 | -------------------------------------------------------------------------------- /plansys2_msgs/srv/GetPlanArray.srv: -------------------------------------------------------------------------------- 1 | string domain 2 | string problem 3 | --- 4 | bool success 5 | plansys2_msgs/PlanArray plan_array 6 | string error_info 7 | -------------------------------------------------------------------------------- /plansys2_msgs/srv/GetProblem.srv: -------------------------------------------------------------------------------- 1 | std_msgs/Empty request 2 | --- 3 | builtin_interfaces/Time stamp 4 | bool success 5 | string problem 6 | string error_info 7 | -------------------------------------------------------------------------------- /plansys2_msgs/srv/GetProblemGoal.srv: -------------------------------------------------------------------------------- 1 | std_msgs/Empty request 2 | --- 3 | bool success 4 | plansys2_msgs/Tree tree 5 | string error_info 6 | -------------------------------------------------------------------------------- /plansys2_msgs/srv/GetProblemInstanceDetails.srv: -------------------------------------------------------------------------------- 1 | string instance 2 | --- 3 | bool success 4 | plansys2_msgs/Param instance 5 | string error_info 6 | -------------------------------------------------------------------------------- /plansys2_msgs/srv/GetProblemInstances.srv: -------------------------------------------------------------------------------- 1 | std_msgs/Empty request 2 | --- 3 | bool success 4 | plansys2_msgs/Param[] instances 5 | string error_info 6 | -------------------------------------------------------------------------------- /plansys2_msgs/srv/GetStates.srv: -------------------------------------------------------------------------------- 1 | std_msgs/Empty request 2 | --- 3 | bool success 4 | plansys2_msgs/Node[] states 5 | string error_info 6 | -------------------------------------------------------------------------------- /plansys2_msgs/srv/IsProblemGoalSatisfied.srv: -------------------------------------------------------------------------------- 1 | plansys2_msgs/Tree tree 2 | --- 3 | bool success 4 | bool satisfied 5 | string error_info 6 | -------------------------------------------------------------------------------- /plansys2_msgs/srv/RemoveProblemGoal.srv: -------------------------------------------------------------------------------- 1 | std_msgs/Empty request 2 | --- 3 | bool success 4 | string error_info -------------------------------------------------------------------------------- /plansys2_msgs/srv/ValidateDomain.srv: -------------------------------------------------------------------------------- 1 | string domain 2 | --- 3 | bool success 4 | string error_info 5 | -------------------------------------------------------------------------------- /plansys2_pddl_parser/LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2019, Intelligent Robotics Labs 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /plansys2_pddl_parser/include/plansys2_pddl_parser/CondIter.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2024 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 | #pragma once 15 | 16 | #include 17 | #include 18 | 19 | #include "plansys2_pddl_parser/Condition.hpp" 20 | 21 | namespace parser 22 | { 23 | namespace pddl 24 | { 25 | 26 | typedef std::list> CondList; 27 | 28 | class CondIter : public std::iterator 29 | { 30 | public: 31 | CondList condStack; 32 | 33 | explicit CondIter(Condition * c) 34 | { 35 | condStack.push_back(std::make_pair(c, 0)); 36 | (*this)++; 37 | } 38 | 39 | explicit CondIter(const CondIter & ci) 40 | : condStack(ci.condStack) {} 41 | 42 | MyIterator & operator++() 43 | { 44 | while (condStack().size() && condStack.last().done()) {condStack.pop_back();} 45 | 46 | return *this; 47 | } 48 | 49 | bool hasNext() {return condStack.size();} 50 | 51 | Condition * operator*() {return condStack.last();} 52 | }; 53 | 54 | } // namespace pddl 55 | } // namespace parser 56 | -------------------------------------------------------------------------------- /plansys2_pddl_parser/include/plansys2_pddl_parser/EitherType.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2024 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 | #pragma once 15 | 16 | #include 17 | 18 | #include "plansys2_pddl_parser/Type.hpp" 19 | 20 | namespace parser 21 | { 22 | namespace pddl 23 | { 24 | 25 | class EitherType : public Type 26 | { 27 | public: 28 | explicit EitherType(const std::string & s) 29 | : Type(s) {} 30 | 31 | explicit EitherType(const EitherType * t) 32 | : Type(t) {} 33 | 34 | std::string getName() const 35 | { 36 | std::string out = "either"; 37 | for (unsigned i = 0; i < subtypes.size(); ++i) {out += "_" + subtypes[i]->getName();} 38 | return out; 39 | } 40 | 41 | void PDDLPrint(std::ostream & s) const override {} 42 | 43 | Type * copy() {return new EitherType(this);} 44 | }; 45 | 46 | } // namespace pddl 47 | } // namespace parser 48 | -------------------------------------------------------------------------------- /plansys2_pddl_parser/include/plansys2_pddl_parser/Exists.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2024 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 | #pragma once 15 | 16 | #include 17 | #include 18 | #include 19 | 20 | #include "plansys2_msgs/msg/node.hpp" 21 | #include "plansys2_msgs/msg/tree.hpp" 22 | #include "plansys2_pddl_parser/ParamCond.hpp" 23 | 24 | namespace parser 25 | { 26 | namespace pddl 27 | { 28 | 29 | class Exists : public ParamCond 30 | { 31 | public: 32 | Condition * cond; 33 | 34 | Exists() 35 | : cond(0) {} 36 | 37 | Exists(const Exists * e, Domain & d) 38 | : ParamCond(e), cond(0) 39 | { 40 | if (e->cond) {cond = e->cond->copy(d);} 41 | } 42 | 43 | ~Exists() 44 | { 45 | if (cond) {delete cond;} 46 | } 47 | 48 | void print(std::ostream & s) const 49 | { 50 | s << "Exists" << params << ":\n"; 51 | cond->print(s); 52 | } 53 | 54 | void PDDLPrint( 55 | std::ostream & s, unsigned indent, const TokenStruct & ts, 56 | const Domain & d) const override; 57 | 58 | plansys2_msgs::msg::Node::SharedPtr getTree( 59 | plansys2_msgs::msg::Tree & tree, const Domain & d, 60 | const std::vector & replace = {}) const override; 61 | 62 | void parse(Stringreader & f, TokenStruct & ts, Domain & d); 63 | 64 | void addParams(int m, unsigned n) {cond->addParams(m, n);} 65 | 66 | Condition * copy(Domain & d) {return new Exists(this, d);} 67 | }; 68 | 69 | } // namespace pddl 70 | } // namespace parser 71 | -------------------------------------------------------------------------------- /plansys2_pddl_parser/include/plansys2_pddl_parser/Function.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2024 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 | #pragma once 15 | 16 | #include 17 | #include 18 | 19 | #include "plansys2_msgs/msg/node.hpp" 20 | #include "plansys2_msgs/msg/tree.hpp" 21 | #include "plansys2_pddl_parser/Lifted.hpp" 22 | 23 | namespace parser 24 | { 25 | namespace pddl 26 | { 27 | 28 | class Function : public Lifted 29 | { 30 | public: 31 | int returnType; 32 | 33 | Function() 34 | : Lifted(), returnType(-1) {} 35 | 36 | explicit Function(const std::string & s, int type = -1) 37 | : Lifted(s), returnType(type) {} 38 | 39 | explicit Function(const ParamCond * c) 40 | : Lifted(c), returnType(-1) {} 41 | 42 | void PDDLPrint( 43 | std::ostream & s, unsigned indent, const TokenStruct & ts, 44 | const Domain & d) const override; 45 | 46 | plansys2_msgs::msg::Node::SharedPtr getTree( 47 | plansys2_msgs::msg::Tree & tree, const Domain & d, 48 | const std::vector & replace = {}) const override; 49 | 50 | void parse(Stringreader & f, TokenStruct & ts, Domain & d); 51 | }; 52 | 53 | } // namespace pddl 54 | } // namespace parser 55 | -------------------------------------------------------------------------------- /plansys2_pddl_parser/include/plansys2_pddl_parser/GroundFunc.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2024 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 | #pragma once 15 | 16 | #include 17 | #include 18 | 19 | #include "plansys2_msgs/msg/node.hpp" 20 | #include "plansys2_msgs/msg/tree.hpp" 21 | #include "plansys2_pddl_parser/TypeGround.hpp" 22 | 23 | namespace parser 24 | { 25 | namespace pddl 26 | { 27 | 28 | template 29 | class GroundFunc : public TypeGround 30 | { 31 | public: 32 | T value; 33 | 34 | GroundFunc() 35 | : TypeGround(), value(0) {} 36 | 37 | explicit GroundFunc(Lifted * l, const T & val = T(0)) 38 | : TypeGround(l), value(val) {} 39 | 40 | void PDDLPrint( 41 | std::ostream & s, unsigned indent, const TokenStruct & ts, 42 | const Domain & d) const override; 43 | 44 | plansys2_msgs::msg::Node::SharedPtr getTree( 45 | plansys2_msgs::msg::Tree & tree, const Domain & d, 46 | const std::vector & replace = {}) const override; 47 | 48 | void print(std::ostream & stream) const {stream << name << params << value << "\n";} 49 | 50 | void parse(Stringreader & f, TokenStruct & ts, Domain & d); 51 | }; 52 | 53 | } // namespace pddl 54 | } // namespace parser 55 | -------------------------------------------------------------------------------- /plansys2_pddl_parser/include/plansys2_pddl_parser/Imply.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2024 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 | #pragma once 15 | 16 | #include 17 | #include 18 | #include 19 | 20 | #include "plansys2_msgs/msg/node.hpp" 21 | #include "plansys2_msgs/msg/tree.hpp" 22 | #include "plansys2_pddl_parser/ParamCond.hpp" 23 | 24 | namespace parser 25 | { 26 | namespace pddl 27 | { 28 | 29 | class Imply : public ParamCond 30 | { 31 | public: 32 | Condition * cond; 33 | 34 | Imply() 35 | : cond(0) {} 36 | 37 | Imply(const Imply * f, Domain & d) 38 | : ParamCond(f), cond(0) 39 | { 40 | if (f->cond) {cond = f->cond->copy(d);} 41 | } 42 | 43 | ~Imply() 44 | { 45 | if (cond) {delete cond;} 46 | } 47 | 48 | void print(std::ostream & s) const 49 | { 50 | s << "Imply" << params << ":\n"; 51 | if (cond) {cond->print(s);} 52 | } 53 | 54 | void PDDLPrint( 55 | std::ostream & s, unsigned indent, const TokenStruct & ts, 56 | const Domain & d) const override; 57 | 58 | plansys2_msgs::msg::Node::SharedPtr getTree( 59 | plansys2_msgs::msg::Tree & tree, const Domain & d, 60 | const std::vector & replace = {}) const override; 61 | 62 | void parse(Stringreader & f, TokenStruct & ts, Domain & d); 63 | 64 | void addParams(int m, unsigned n) {cond->addParams(m, n);} 65 | 66 | Condition * copy(Domain & d) {return new Imply(this, d);} 67 | }; 68 | 69 | } // namespace pddl 70 | } // namespace parser 71 | -------------------------------------------------------------------------------- /plansys2_pddl_parser/include/plansys2_pddl_parser/Lifted.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2024 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 | #pragma once 15 | 16 | #include 17 | #include 18 | #include 19 | 20 | #include "plansys2_msgs/msg/node.hpp" 21 | #include "plansys2_msgs/msg/tree.hpp" 22 | #include "plansys2_pddl_parser/ParamCond.hpp" 23 | 24 | namespace parser 25 | { 26 | namespace pddl 27 | { 28 | 29 | class Lifted : public ParamCond 30 | { 31 | public: 32 | Lifted() {} 33 | 34 | explicit Lifted(const std::string & s) 35 | : ParamCond(s) {} 36 | 37 | explicit Lifted(const ParamCond * c) 38 | : ParamCond(c) {} 39 | 40 | void PDDLPrint( 41 | std::ostream & s, unsigned indent, const TokenStruct & ts, 42 | const Domain & d) const override; 43 | 44 | plansys2_msgs::msg::Node::SharedPtr getTree( 45 | plansys2_msgs::msg::Tree & tree, const Domain & d, 46 | const std::vector & replace = {}) const override; 47 | 48 | void parse(Stringreader & f, TokenStruct & ts, Domain & d); 49 | 50 | void addParams(int m, unsigned n) {} 51 | 52 | Condition * copy(Domain & d) {return new Lifted(this);} 53 | }; 54 | 55 | typedef std::vector LiftedVec; 56 | 57 | } // namespace pddl 58 | } // namespace parser 59 | -------------------------------------------------------------------------------- /plansys2_pddl_parser/include/plansys2_pddl_parser/ParamCond.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2024 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 | #pragma once 15 | 16 | #include 17 | #include 18 | 19 | #include "plansys2_pddl_parser/Basic.hpp" 20 | #include "plansys2_pddl_parser/Condition.hpp" 21 | 22 | namespace parser 23 | { 24 | namespace pddl 25 | { 26 | 27 | // This is necessary for adl 28 | using ::operator<<; 29 | 30 | class ParamCond : public Condition 31 | { 32 | public: 33 | std::string name; 34 | IntVec params; 35 | 36 | ParamCond() {} 37 | 38 | explicit ParamCond(const std::string & s, const IntVec & p = IntVec()) 39 | : name(s), params(p) {} 40 | 41 | explicit ParamCond(const ParamCond * c) 42 | : name(c->name), params(c->params) {} 43 | 44 | std::string c_str() const {return name;} 45 | 46 | void print(std::ostream & stream) const {stream << name << params << "\n";} 47 | 48 | void printParams( 49 | unsigned first, std::ostream & s, TokenStruct & ts, const Domain & d) const; 50 | }; 51 | 52 | typedef std::vector ParamCondVec; 53 | 54 | } // namespace pddl 55 | } // namespace parser 56 | -------------------------------------------------------------------------------- /plansys2_pddl_parser/include/plansys2_pddl_parser/Task.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2024 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 | #pragma once 15 | 16 | #include 17 | #include 18 | #include 19 | 20 | #include "plansys2_msgs/msg/node.hpp" 21 | #include "plansys2_msgs/msg/tree.hpp" 22 | #include "plansys2_pddl_parser/ParamCond.hpp" 23 | 24 | namespace parser 25 | { 26 | namespace pddl 27 | { 28 | 29 | class Task : public ParamCond 30 | { 31 | public: 32 | Task() {} 33 | 34 | explicit Task(const std::string & s) 35 | : ParamCond(s) {} 36 | 37 | explicit Task(const ParamCond * c) 38 | : ParamCond(c) {} 39 | 40 | void PDDLPrint( 41 | std::ostream & s, unsigned indent, const TokenStruct & ts, 42 | const Domain & d) const override 43 | { 44 | } 45 | 46 | plansys2_msgs::msg::Node::SharedPtr getTree( 47 | plansys2_msgs::msg::Tree & tree, const Domain & d, 48 | const std::vector & replace = {}) const override 49 | { 50 | throw UnsupportedConstruct("Task"); 51 | } 52 | 53 | void parse(Stringreader & f, TokenStruct & ts, Domain & d) {} 54 | 55 | void addParams(int m, unsigned n) {} 56 | 57 | Condition * copy(Domain & d) {return new Task(this);} 58 | }; 59 | 60 | typedef std::vector TaskVec; 61 | 62 | } // namespace pddl 63 | } // namespace parser 64 | -------------------------------------------------------------------------------- /plansys2_pddl_parser/include/plansys2_pddl_parser/TypeGround.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2024 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 | #pragma once 15 | 16 | #include 17 | #include 18 | 19 | #include "plansys2_msgs/msg/node.hpp" 20 | #include "plansys2_msgs/msg/tree.hpp" 21 | #include "plansys2_pddl_parser/Ground.hpp" 22 | 23 | namespace parser 24 | { 25 | namespace pddl 26 | { 27 | 28 | class TypeGround : public Ground 29 | { 30 | public: 31 | TypeGround() 32 | : Ground() {} 33 | 34 | explicit TypeGround(Lifted * l, const IntVec & p = IntVec()) 35 | : Ground(l, p) {} 36 | 37 | // TypeGround( const TypeGround * tg ) 38 | // : Ground( tg ) {} 39 | 40 | void PDDLPrint( 41 | std::ostream & s, unsigned indent, const TokenStruct & ts, 42 | const Domain & d) const override; 43 | 44 | plansys2_msgs::msg::Node::SharedPtr getTree( 45 | plansys2_msgs::msg::Tree & tree, const Domain & d, 46 | const std::vector & replace = {}) const override; 47 | 48 | void insert(Domain & d, const StringVec & v); 49 | 50 | void parse(Stringreader & f, TokenStruct & ts, Domain & d); 51 | }; 52 | 53 | typedef std::vector TypeGroundVec; 54 | 55 | } // namespace pddl 56 | } // namespace parser 57 | -------------------------------------------------------------------------------- /plansys2_pddl_parser/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | plansys2_pddl_parser 5 | 3.0.0 6 | 7 | This package contains a library for parsing PDDL domains and problems. 8 | 9 | This package derives from the work of Anders Jonsson, contained in https://github.com/wisdompoet/universal-pddl-parser.git 10 | with many modifications by Francisco Martin: 11 | * ROS2 packaging 12 | * Source code structure refactor 13 | * CMakeLists.txt for cmake compilation 14 | * Reading from String instead of files 15 | * Licensing 16 | 17 | 18 | Francisco Martin Rico 19 | 20 | Apache License, Version 2.0 21 | 22 | Anders Jonsson 23 | Francisco Martin 24 | 25 | ament_cmake 26 | 27 | plansys2_msgs 28 | 29 | ament_lint_common 30 | ament_lint_auto 31 | ament_cmake_gtest 32 | ament_index_cpp 33 | rclcpp 34 | 35 | 36 | ament_cmake 37 | 38 | 39 | -------------------------------------------------------------------------------- /plansys2_pddl_parser/src/parser.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2024 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 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | #include "plansys2_pddl_parser/Instance.hpp" 20 | 21 | int main(int argc, char * argv[]) 22 | { 23 | if (argc < 3) { 24 | std::cout << "Usage: parser \n"; 25 | exit(1); 26 | } 27 | std::ifstream domain_ifs(argv[1]); 28 | std::string domain_str( 29 | (std::istreambuf_iterator(domain_ifs)), std::istreambuf_iterator()); 30 | 31 | std::ifstream instance_ifs(argv[2]); 32 | std::string instance_str( 33 | (std::istreambuf_iterator(instance_ifs)), std::istreambuf_iterator()); 34 | 35 | // Read multiagent domain and instance 36 | parser::pddl::Domain domain(domain_str); 37 | 38 | std::cout << domain << std::endl; 39 | 40 | parser::pddl::Instance instance(domain, instance_str); 41 | 42 | std::cout << instance << std::endl; 43 | } 44 | -------------------------------------------------------------------------------- /plansys2_pddl_parser/src/plansys2_pddl_parser/Function.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2024 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 | #include "plansys2_pddl_parser/Domain.hpp" 15 | 16 | namespace parser 17 | { 18 | namespace pddl 19 | { 20 | 21 | void Function::PDDLPrint( 22 | std::ostream & s, unsigned indent, const TokenStruct & ts, const Domain & d) const 23 | { 24 | Lifted::PDDLPrint(s, indent, ts, d); 25 | if (returnType >= 0) {s << " - " << d.types[returnType]->name;} 26 | } 27 | 28 | plansys2_msgs::msg::Node::SharedPtr Function::getTree( 29 | plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace) const 30 | { 31 | throw UnsupportedConstruct("Function"); 32 | } 33 | 34 | void Function::parse(Stringreader & f, TokenStruct & ts, Domain & d) 35 | { 36 | Lifted::parse(f, ts, d); 37 | 38 | f.next(); 39 | if (f.getChar() == '-') { 40 | f.assert_token("-"); 41 | std::string s = f.getToken(); 42 | if (s != "number") { 43 | f.c -= s.size(); 44 | returnType = d.types.index(f.getToken(d.types)); 45 | } 46 | } 47 | } 48 | 49 | } // namespace pddl 50 | } // namespace parser 51 | -------------------------------------------------------------------------------- /plansys2_pddl_parser/src/plansys2_pddl_parser/Lifted.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2024 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 | #include "plansys2_pddl_parser/Domain.hpp" 15 | 16 | namespace parser 17 | { 18 | namespace pddl 19 | { 20 | 21 | void Lifted::PDDLPrint( 22 | std::ostream & s, unsigned indent, const TokenStruct & ts, const Domain & d) const 23 | { 24 | tabindent(s, indent); 25 | s << "( " << name; 26 | for (unsigned i = 0; i < params.size(); ++i) { 27 | if (ts.size()) { 28 | s << ts[i]; 29 | } else { 30 | s << " ?" << d.types[params[i]]->getName() << i; 31 | } 32 | if (d.typed) {s << " - " << d.types[params[i]]->name;} 33 | } 34 | s << " )"; 35 | } 36 | 37 | plansys2_msgs::msg::Node::SharedPtr Lifted::getTree( 38 | plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace) const 39 | { 40 | throw UnsupportedConstruct("Lifted"); 41 | } 42 | 43 | void Lifted::parse(Stringreader & f, TokenStruct & ts, Domain & d) 44 | { 45 | TokenStruct lstruct = f.parseTypedList(true, d.types); 46 | params = d.convertTypes(lstruct.types); 47 | } 48 | 49 | } // namespace pddl 50 | } // namespace parser 51 | -------------------------------------------------------------------------------- /plansys2_pddl_parser/src/plansys2_pddl_parser/Not.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2024 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 | #include "plansys2_pddl_parser/Domain.hpp" 15 | 16 | namespace parser 17 | { 18 | namespace pddl 19 | { 20 | 21 | void Not::PDDLPrint( 22 | std::ostream & s, unsigned indent, const TokenStruct & ts, const Domain & d) const 23 | { 24 | tabindent(s, indent); 25 | s << "( not "; 26 | if (cond) {cond->PDDLPrint(s, 0, ts, d);} 27 | s << " )"; 28 | } 29 | 30 | plansys2_msgs::msg::Node::SharedPtr Not::getTree( 31 | plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace) const 32 | { 33 | plansys2_msgs::msg::Node::SharedPtr node = std::make_shared(); 34 | node->node_type = plansys2_msgs::msg::Node::NOT; 35 | node->node_id = tree.nodes.size(); 36 | tree.nodes.push_back(*node); 37 | 38 | if (cond) { 39 | plansys2_msgs::msg::Node::SharedPtr child = cond->getTree(tree, d, replace); 40 | tree.nodes[node->node_id].children.push_back(child->node_id); 41 | } 42 | 43 | return node; 44 | } 45 | 46 | void Not::parse(Stringreader & f, TokenStruct & ts, Domain & d) 47 | { 48 | f.next(); 49 | f.assert_token("("); 50 | 51 | cond = d.createCondition(f); 52 | 53 | if (!cond) { 54 | f.tokenExit(f.getToken()); 55 | } 56 | 57 | cond->parse(f, ts, d); 58 | 59 | f.next(); 60 | f.assert_token(")"); 61 | } 62 | 63 | } // namespace pddl 64 | } // namespace parser 65 | -------------------------------------------------------------------------------- /plansys2_pddl_parser/src/plansys2_pddl_parser/Oneof.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2024 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 | #include "plansys2_pddl_parser/Domain.hpp" 15 | 16 | namespace parser 17 | { 18 | namespace pddl 19 | { 20 | 21 | void Oneof::PDDLPrint( 22 | std::ostream & s, unsigned indent, const TokenStruct & ts, const Domain & d) const 23 | { 24 | tabindent(s, indent); 25 | s << "( oneof\n"; 26 | for (unsigned i = 0; i < conds.size(); ++i) { 27 | conds[i]->PDDLPrint(s, indent + 1, ts, d); 28 | s << "\n"; 29 | } 30 | tabindent(s, indent); 31 | s << ")"; 32 | } 33 | 34 | plansys2_msgs::msg::Node::SharedPtr Oneof::getTree( 35 | plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace) const 36 | { 37 | throw UnsupportedConstruct("Oneof"); 38 | } 39 | 40 | void Oneof::parse(Stringreader & f, TokenStruct & ts, Domain & d) 41 | { 42 | for (f.next(); f.getChar() != ')'; f.next()) { 43 | f.assert_token("("); 44 | Condition * condition = d.createCondition(f); 45 | condition->parse(f, ts, d); 46 | conds.push_back(condition); 47 | } 48 | ++f.c; 49 | } 50 | 51 | } // namespace pddl 52 | } // namespace parser 53 | -------------------------------------------------------------------------------- /plansys2_pddl_parser/src/plansys2_pddl_parser/ParamCond.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2024 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 | #include "plansys2_pddl_parser/Domain.hpp" 15 | 16 | namespace parser 17 | { 18 | namespace pddl 19 | { 20 | 21 | void ParamCond::printParams( 22 | unsigned first, std::ostream & s, TokenStruct & ts, const Domain & d) const 23 | { 24 | s << "("; 25 | for (unsigned i = first; i < params.size(); ++i) { 26 | std::stringstream ss; 27 | ss << "?" << d.types[params[i]]->getName() << ts.size(); 28 | ts.insert(ss.str()); 29 | s << " " << ss.str(); 30 | if (d.typed) {s << " - " << d.types[params[i]]->name;} 31 | } 32 | s << " )\n"; 33 | } 34 | 35 | } // namespace pddl 36 | } // namespace parser 37 | -------------------------------------------------------------------------------- /plansys2_pddl_parser/test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ament_add_gtest(pddl_parser_test pddl_parser_test.cpp TIMEOUT 120) 2 | target_link_libraries(pddl_parser_test 3 | ament_index_cpp::ament_index_cpp 4 | rclcpp::rclcpp 5 | ${PROJECT_NAME} 6 | ) 7 | 8 | install(DIRECTORY 9 | pddl 10 | DESTINATION share/${PROJECT_NAME} 11 | ) 12 | -------------------------------------------------------------------------------- /plansys2_pddl_parser/test/pddl/dom2.pddl: -------------------------------------------------------------------------------- 1 | (define (domain simple-door) 2 | (:requirements :strips :typing :adl :fluents :durative-actions :existential-preconditions) 3 | 4 | (:predicates 5 | (door_open) 6 | (not_door_open) 7 | ) 8 | 9 | (:durative-action open_door 10 | :parameters () 11 | :duration ( = ?duration 1) 12 | :condition (and 13 | (at start (not_door_open)) 14 | ) 15 | :effect (and 16 | (at start (door_open)) 17 | (at start (not (not_door_open))) 18 | ) 19 | ) 20 | 21 | (:durative-action close_door 22 | :parameters () 23 | :duration ( = ?duration 1) 24 | :condition (and 25 | (at start (door_open)) 26 | ) 27 | :effect (and 28 | (at start (not_door_open)) 29 | (at start (not (door_open))) 30 | ) 31 | ) 32 | ) 33 | -------------------------------------------------------------------------------- /plansys2_pddl_parser/test/pddl/prob1.pddl: -------------------------------------------------------------------------------- 1 | (define (problem test) (:domain simple) 2 | (:objects 3 | bot - robot 4 | entrance - room 5 | kitchen - room 6 | bedroom - room 7 | dinning - room 8 | bathroom - room 9 | chargingroom - room 10 | ) 11 | 12 | (:init 13 | (connected entrance dinning) 14 | (connected dinning entrance) 15 | (connected dinning kitchen) 16 | (connected kitchen dinning) 17 | (connected dinning bedroom) 18 | (connected bedroom dinning) 19 | (connected bathroom bedroom) 20 | (connected bedroom bathroom) 21 | (connected chargingroom kitchen) 22 | (connected kitchen chargingroom) 23 | (charging_point_at chargingroom) 24 | (robot_at bot entrance) 25 | (robot_at rob1 entrance) 26 | (= (battery_level bot) 90) 27 | (= (battery_level rob1) 100) 28 | ) 29 | 30 | (:goal (and 31 | (robot_at bot bathroom) 32 | )) 33 | 34 | 35 | ) -------------------------------------------------------------------------------- /plansys2_pddl_parser/test/pddl/prob2.pddl: -------------------------------------------------------------------------------- 1 | (define (problem open-the-door) 2 | (:domain simple-door) 3 | (:init 4 | (not_door_open) 5 | ) 6 | (:goal 7 | (and (door_open)) 8 | ) 9 | ) 10 | -------------------------------------------------------------------------------- /plansys2_planner/README.md: -------------------------------------------------------------------------------- 1 | # Planner 2 | 3 | The Planner module is responsible for creating plans. By default, it uses [popf](https://github.com/fmrico/popf), that is a PDDL solver. 4 | 5 | This module is very simple, as its only task is calling the popf binary and parsing the result. 6 | 7 | The main class is [`plansys2::PlannerNode`](include/include/plansys2_planner/PlannerNode.hpp), which is instantiated from [`planner_node.cpp`](src/planner_node.cpp). `plansys2::PlannerNode` is a also `rclcpp_lifecycle::LifecycleNode`, but currently the functionality is in the active phase. 8 | 9 | Plan solvers are specified in the `plan_solver_plugins` parameter. In case of more than one specified, the first one will be used. If this parameter is not specified, POPF will be used by default. 10 | 11 | ## Services 12 | 13 | - `/planner/get_plan` [[`plansys2_msgs::srv::GetPlan`](../plansys2_msgs/srv/GetPlan.srv)] 14 | -------------------------------------------------------------------------------- /plansys2_planner/launch/planner_launch.py: -------------------------------------------------------------------------------- 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 | from launch import LaunchDescription 16 | from launch.actions import DeclareLaunchArgument 17 | from launch.substitutions import LaunchConfiguration 18 | from launch_ros.actions import Node 19 | 20 | 21 | def generate_launch_description(): 22 | namespace = LaunchConfiguration('namespace') 23 | params_file = LaunchConfiguration('params_file') 24 | 25 | declare_namespace_cmd = DeclareLaunchArgument( 26 | 'namespace', 27 | default_value='', 28 | description='Namespace') 29 | 30 | # Specify the actions 31 | planner_cmd = Node( 32 | package='plansys2_planner', 33 | executable='planner_node', 34 | name='planner', 35 | namespace=namespace, 36 | output='screen', 37 | parameters=[params_file]) 38 | 39 | # Create the launch description and populate 40 | ld = LaunchDescription() 41 | 42 | ld.add_action(declare_namespace_cmd) 43 | 44 | # Declare the launch options 45 | ld.add_action(planner_cmd) 46 | 47 | return ld 48 | -------------------------------------------------------------------------------- /plansys2_planner/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | plansys2_planner 5 | 3.0.0 6 | 7 | This package contains the PDDL-based Planner module for the ROS2 Planning System 8 | 9 | Francisco Martin Rico 10 | 11 | Apache License, Version 2.0 12 | 13 | ament_cmake 14 | 15 | rclcpp 16 | rclcpp_lifecycle 17 | ament_index_cpp 18 | plansys2_core 19 | plansys2_pddl_parser 20 | plansys2_msgs 21 | plansys2_domain_expert 22 | plansys2_problem_expert 23 | plansys2_popf_plan_solver 24 | pluginlib 25 | lifecycle_msgs 26 | 27 | ament_lint_common 28 | ament_lint_auto 29 | ament_cmake_gtest 30 | ros2run 31 | 32 | 33 | ament_cmake 34 | 35 | 36 | -------------------------------------------------------------------------------- /plansys2_planner/src/planner_node.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 | 17 | #include "plansys2_planner/PlannerNode.hpp" 18 | #include "rclcpp/rclcpp.hpp" 19 | 20 | int main(int argc, char ** argv) 21 | { 22 | rclcpp::init(argc, argv); 23 | auto node = std::make_shared(); 24 | 25 | rclcpp::spin(node->get_node_base_interface()); 26 | 27 | rclcpp::shutdown(); 28 | 29 | return 0; 30 | } 31 | -------------------------------------------------------------------------------- /plansys2_planner/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_planner/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 | (: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 | :effect (and 32 | (at start(not(robot_at ?r ?r1))) 33 | (at end(robot_at ?r ?r2)) 34 | ) 35 | ) 36 | 37 | (:durative-action talk 38 | :parameters (?r - robot ?p - person ?m - message) 39 | :duration ( = ?duration 5) 40 | :condition (and 41 | (over all(robot_near_person ?r ?p)) 42 | ) 43 | :effect (and 44 | (at end(robot_talk ?r ?m ?p)) 45 | ) 46 | ) 47 | 48 | (:durative-action approach 49 | :parameters (?r - robot ?ro - room ?p - person) 50 | :duration ( = ?duration 5) 51 | :condition (and 52 | (over all(robot_at ?r ?ro)) 53 | (over all(person_at ?p ?ro)) 54 | ) 55 | :effect (and 56 | (at end(robot_near_person ?r ?p)) 57 | ) 58 | ) 59 | 60 | 61 | 62 | );; end Domain ;;;;;;;;;;;;;;;;;;;;;;;; 63 | -------------------------------------------------------------------------------- /plansys2_planner/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_planner/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_planner/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_planner/test/pddl/problem_simple_constants_1.pddl: -------------------------------------------------------------------------------- 1 | ( define ( problem problem_1 ) 2 | ( :domain plansys2 ) 3 | ( :objects 4 | m1 - message 5 | kitchen bedroom - room 6 | ) 7 | ( :init 8 | ( robot_at leia kitchen ) 9 | ( person_at jack bedroom ) 10 | ) 11 | ( :goal 12 | ( and 13 | ( robot_talk leia m1 jack ) 14 | )) 15 | ) 16 | -------------------------------------------------------------------------------- /plansys2_planner/test/pddl/problem_simple_constants_2.pddl: -------------------------------------------------------------------------------- 1 | ( define ( problem problem_1 ) 2 | ( :domain plansys2 ) 3 | ( :objects 4 | jack john - person 5 | m1 - message 6 | leia lema - robot 7 | kitchen bedroom - room 8 | ) 9 | ( :init 10 | ( robot_at leia kitchen ) 11 | ( person_at jack bedroom ) 12 | ) 13 | ( :goal 14 | ( and 15 | ( robot_talk leia m1 jack ) 16 | ) 17 | ) 18 | ) 19 | -------------------------------------------------------------------------------- /plansys2_planner/test/unit/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ament_add_gtest(planner_test planner_test.cpp) 2 | target_link_libraries(planner_test 3 | ${PROJECT_NAME} 4 | ament_index_cpp::ament_index_cpp 5 | dl 6 | plansys2_pddl_parser::plansys2_pddl_parser 7 | ) 8 | target_compile_definitions(planner_test PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") -------------------------------------------------------------------------------- /plansys2_popf_plan_solver/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | project(plansys2_popf_plan_solver) 4 | 5 | find_package(ament_cmake REQUIRED) 6 | find_package(rclcpp REQUIRED) 7 | find_package(plansys2_core REQUIRED) 8 | find_package(pluginlib REQUIRED) 9 | 10 | add_library(${PROJECT_NAME} SHARED 11 | src/plansys2_popf_plan_solver/popf_plan_solver.cpp 12 | ) 13 | target_include_directories(${PROJECT_NAME} PUBLIC 14 | "$" 15 | "$" 16 | "$") 17 | target_link_libraries(${PROJECT_NAME} 18 | PUBLIC 19 | plansys2_core::plansys2_core 20 | PRIVATE 21 | pluginlib::pluginlib 22 | rclcpp::rclcpp 23 | ) 24 | 25 | target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") 26 | 27 | pluginlib_export_plugin_description_file(plansys2_core plansys2_popf_plan_solver_plugin.xml) 28 | 29 | install(DIRECTORY include/ 30 | DESTINATION include/${PROJECT_NAME} 31 | ) 32 | 33 | install(FILES plansys2_popf_plan_solver_plugin.xml 34 | DESTINATION share/${PROJECT_NAME} 35 | ) 36 | 37 | install(TARGETS ${PROJECT_NAME} 38 | EXPORT export_${PROJECT_NAME} 39 | ARCHIVE DESTINATION lib 40 | LIBRARY DESTINATION lib 41 | RUNTIME DESTINATION bin 42 | ) 43 | 44 | if(BUILD_TESTING) 45 | find_package(ament_lint_auto REQUIRED) 46 | ament_lint_auto_find_test_dependencies() 47 | 48 | find_package(ament_cmake_gtest REQUIRED) 49 | find_package(ament_index_cpp REQUIRED) 50 | add_subdirectory(test) 51 | endif() 52 | 53 | ament_export_include_directories("include/${PROJECT_NAME}") 54 | ament_export_libraries(${PROJECT_NAME}) 55 | ament_export_targets(export_${PROJECT_NAME}) 56 | ament_export_dependencies( 57 | rclcpp 58 | ament_index_cpp 59 | plansys2_core 60 | pluginlib 61 | ) 62 | 63 | ament_package() 64 | -------------------------------------------------------------------------------- /plansys2_popf_plan_solver/README.md: -------------------------------------------------------------------------------- 1 | # POPF Plan solver 2 | 3 | This package contains a plan solver that uses [popf](https://github.com/fmrico/popf) for solving PDDL plans. 4 | -------------------------------------------------------------------------------- /plansys2_popf_plan_solver/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | plansys2_popf_plan_solver 5 | 3.0.0 6 | 7 | This package contains the PDDL-based Planner module for the ROS2 Planning System 8 | 9 | Francisco Martin Rico 10 | 11 | Apache License, Version 2.0 12 | 13 | ament_cmake 14 | 15 | rclcpp 16 | 17 | plansys2_core 18 | pluginlib 19 | 20 | popf 21 | 22 | ament_lint_common 23 | ament_lint_auto 24 | ament_cmake_gtest 25 | ros2run 26 | ament_index_cpp 27 | 28 | 29 | ament_cmake 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /plansys2_popf_plan_solver/plansys2_popf_plan_solver_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /plansys2_popf_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_popf_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_popf_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_popf_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 | (: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 | :effect (and 32 | (at start(not(robot_at ?r ?r1))) 33 | (at end(robot_at ?r ?r2)) 34 | ) 35 | ) 36 | 37 | (:durative-action talk 38 | :parameters (?r - robot ?from ?p - person ?m - message) 39 | :duration ( = ?duration 5) 40 | :condition (and 41 | (over all(robot_near_person ?r ?p)) 42 | ) 43 | :effect (and 44 | (at end(robot_talk ?r ?m ?p)) 45 | ) 46 | ) 47 | 48 | (:durative-action approach 49 | :parameters (?r - robot ?ro - room ?p - person) 50 | :duration ( = ?duration 5) 51 | :condition (and 52 | (over all(robot_at ?r ?ro)) 53 | (over all(person_at ?p ?ro)) 54 | ) 55 | :effect (and 56 | (at end(robot_near_person ?r ?p)) 57 | ) 58 | ) 59 | 60 | 61 | 62 | );; end Domain ;;;;;;;;;;;;;;;;;;;;;;;; 63 | -------------------------------------------------------------------------------- /plansys2_popf_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_popf_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_popf_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_popf_plan_solver/test/unit/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ament_add_gtest(popf_test popf_test.cpp) 2 | target_link_libraries(popf_test 3 | ${PROJECT_NAME} 4 | ament_index_cpp::ament_index_cpp 5 | dl 6 | pluginlib::pluginlib 7 | ) 8 | target_compile_definitions(popf_test PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") -------------------------------------------------------------------------------- /plansys2_problem_expert/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | plansys2_problem_expert 5 | 3.0.0 6 | 7 | This package contains the Problem Expert module for the ROS2 Planning System 8 | 9 | Francisco Martin Rico 10 | 11 | Apache License, Version 2.0 12 | 13 | ament_cmake 14 | 15 | rclcpp 16 | rclcpp_lifecycle 17 | ament_index_cpp 18 | plansys2_pddl_parser 19 | plansys2_msgs 20 | plansys2_domain_expert 21 | std_msgs 22 | lifecycle_msgs 23 | 24 | ament_lint_common 25 | ament_lint_auto 26 | ament_cmake_gtest 27 | 28 | 29 | ament_cmake 30 | 31 | 32 | -------------------------------------------------------------------------------- /plansys2_problem_expert/src/problem_expert_node.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 | 17 | #include "plansys2_problem_expert/ProblemExpertNode.hpp" 18 | #include "rclcpp/rclcpp.hpp" 19 | 20 | int main(int argc, char ** argv) 21 | { 22 | rclcpp::init(argc, argv); 23 | auto node = std::make_shared(); 24 | rclcpp::spin(node->get_node_base_interface()); 25 | rclcpp::shutdown(); 26 | 27 | return 0; 28 | } 29 | -------------------------------------------------------------------------------- /plansys2_problem_expert/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_problem_expert/test/pddl/domain_exists.pddl: -------------------------------------------------------------------------------- 1 | (define (domain simple) 2 | (:requirements :strips :typing :adl :fluents :durative-actions :existential-preconditions) 3 | (:types 4 | robot 5 | room 6 | ) 7 | 8 | (:predicates 9 | (robot_at ?r - robot ?ro - room) 10 | (connected ?ro1 ?ro2 - room) 11 | (battery_full ?r - robot) 12 | (charging_point_at ?ro - room) 13 | ) 14 | 15 | (:constants 16 | rob1 rob2 - robot 17 | bedroom bathroom - room 18 | ) 19 | 20 | (:functions 21 | (battery_level ?r - robot) 22 | ) 23 | 24 | (:action action_test 25 | :parameters (?r - robot) 26 | :precondition (and 27 | (exists (?ro) 28 | (and 29 | (robot_at ?r ?ro) 30 | (charging_point_at ?ro) 31 | ) 32 | ) 33 | (and (> (battery_level ?r) 1) (< (battery_level ?r) 200)) 34 | ) 35 | :effect (and 36 | (battery_full ?r) 37 | ) 38 | ) 39 | 40 | (:action action_test2 41 | :parameters (?r - robot) 42 | :precondition 43 | (exists (?ro1 ?ro2) 44 | (and 45 | (robot_at ?r ?ro1) 46 | (connected ?ro1 ?ro2) 47 | ) 48 | ) 49 | :effect (and ) 50 | ) 51 | 52 | 53 | ) 54 | -------------------------------------------------------------------------------- /plansys2_problem_expert/test/pddl/problem_charging.pddl: -------------------------------------------------------------------------------- 1 | (define (problem charging-problem) 2 | (:domain charging) 3 | 4 | ;; Instantiate the objects. 5 | (:objects 6 | r2d2 - robot 7 | wp_control - waypoint 8 | wp1 wp2 wp3 wp4 - waypoint 9 | ) 10 | 11 | (:init 12 | ; Define the initial state predicates. 13 | (robot_at r2d2 wp_control) 14 | (charger_at wp3) 15 | 16 | (connected wp_control wp1) 17 | (connected wp1 wp_control) 18 | (connected wp_control wp2) 19 | (connected wp2 wp_control) 20 | (connected wp_control wp3) 21 | (connected wp3 wp_control) 22 | (connected wp_control wp4) 23 | (connected wp4 wp_control) 24 | 25 | ; Define static functions 26 | (= (speed r2d2) 3) 27 | (= (max_range r2d2) 75) 28 | (= (state_of_charge r2d2) 99) 29 | 30 | (= (distance wp1 wp2) 30) 31 | (= (distance wp1 wp3) 35) 32 | (= (distance wp1 wp4) 40) 33 | (= (distance wp1 wp_control) 45) 34 | (= (distance wp_control wp1) 45) 35 | (= (distance wp4 wp1) 40) 36 | (= (distance wp3 wp1) 35) 37 | (= (distance wp2 wp1) 30) 38 | 39 | (= (distance wp2 wp3) 45) 40 | (= (distance wp2 wp4) 35) 41 | (= (distance wp2 wp_control) 30) 42 | (= (distance wp_control wp2) 30) 43 | (= (distance wp4 wp2) 35) 44 | (= (distance wp3 wp2) 45) 45 | 46 | (= (distance wp3 wp4) 40) 47 | (= (distance wp3 wp_control) 45) 48 | (= (distance wp_control wp3) 45) 49 | (= (distance wp4 wp3) 40) 50 | 51 | (= (distance wp4 wp_control) 40) 52 | (= (distance wp_control wp4) 40) 53 | ) 54 | 55 | (:goal (and 56 | (patrolled wp1) 57 | (patrolled wp2) 58 | (patrolled wp3) 59 | (patrolled wp4) 60 | )) 61 | 62 | (:metric 63 | minimize (total-time) 64 | ) 65 | ) 66 | -------------------------------------------------------------------------------- /plansys2_problem_expert/test/pddl/problem_empty_domain.pddl: -------------------------------------------------------------------------------- 1 | (define (problem simple_1) 2 | (:domain ) 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_problem_expert/test/pddl/problem_simple_1.pddl: -------------------------------------------------------------------------------- 1 | (define (problem simple_1) 2 | (:domain simple) 3 | (:objects 4 | leia - robot 5 | Jack Alice - person 6 | kitchen bedroom - room 7 | m1 - message 8 | ) 9 | (:init 10 | (robot_at leia kitchen) 11 | (person_at Jack bedroom) 12 | (= (room_distance kitchen bedroom) 10) 13 | 14 | 15 | ) 16 | 17 | ;; The goal is to have both packages delivered to their destinations: 18 | (:goal (and 19 | (robot_talk leia m1 Jack) 20 | ) 21 | ) 22 | ) 23 | -------------------------------------------------------------------------------- /plansys2_problem_expert/test/pddl/problem_simple_constants_1.pddl: -------------------------------------------------------------------------------- 1 | ( define ( problem problem_1 ) 2 | ( :domain plansys2 ) 3 | ( :objects 4 | m1 - message 5 | kitchen bedroom - room 6 | ) 7 | ( :init 8 | ( robot_at leia kitchen ) 9 | ( person_at jack bedroom ) 10 | ) 11 | ( :goal 12 | ( and 13 | ( robot_talk leia m1 jack ) 14 | )) 15 | ) 16 | -------------------------------------------------------------------------------- /plansys2_problem_expert/test/pddl/problem_simple_constants_2.pddl: -------------------------------------------------------------------------------- 1 | ( define ( problem problem_1 ) 2 | ( :domain plansys2 ) 3 | ( :objects 4 | jack john - person 5 | m1 - message 6 | leia lema - robot 7 | kitchen bedroom - room 8 | ) 9 | ( :init 10 | ( robot_at leia kitchen ) 11 | ( person_at jack bedroom ) 12 | ) 13 | ( :goal 14 | ( and 15 | ( robot_talk leia m1 jack ) 16 | ) 17 | ) 18 | ) 19 | -------------------------------------------------------------------------------- /plansys2_problem_expert/test/pddl/problem_unexpected_syntax.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 | (= (room_distance kitchen bedroom) 10) 13 | 14 | ;; problem syntax below 15 | ;; too many rooms specified 16 | (is_teleporter_destination kitchen bedroom) 17 | ;; robot erroneously specified 18 | (= (room_distance leia kitchen) 10) 19 | 20 | 21 | ) 22 | 23 | ;; The goal is to have both packages delivered to their destinations: 24 | (:goal (and 25 | (robot_talk leia m1 Jack) 26 | ) 27 | ) 28 | ) 29 | -------------------------------------------------------------------------------- /plansys2_problem_expert/test/unit/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ament_add_gtest(utils_test utils_test.cpp) 2 | target_link_libraries(utils_test 3 | ${PROJECT_NAME} 4 | ament_index_cpp::ament_index_cpp 5 | ) 6 | 7 | ament_add_gtest(problem_expert_test problem_expert_test.cpp) 8 | target_link_libraries(problem_expert_test 9 | ${PROJECT_NAME} 10 | ament_index_cpp::ament_index_cpp 11 | ) 12 | 13 | ament_add_gtest(problem_expert_node_test problem_expert_node_test.cpp) 14 | target_link_libraries(problem_expert_node_test 15 | ${PROJECT_NAME} 16 | ament_index_cpp::ament_index_cpp 17 | ) 18 | -------------------------------------------------------------------------------- /plansys2_support_py/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package plansys2_support_py 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 3.0.0 (2025-06-06) 6 | ------------------ 7 | * Support action performers in Python 8 | * Contributors: Francisco Martín Rico, Marco Roveri 9 | -------------------------------------------------------------------------------- /plansys2_support_py/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | project(plansys2_support_py) 4 | 5 | find_package(ament_cmake REQUIRED) 6 | find_package(ament_cmake_python REQUIRED) 7 | find_package(lifecycle_msgs REQUIRED) 8 | find_package(plansys2_msgs REQUIRED) 9 | 10 | find_package(Python3 REQUIRED COMPONENTS Interpreter Development) 11 | 12 | ament_python_install_package(${PROJECT_NAME}) 13 | 14 | ament_package() -------------------------------------------------------------------------------- /plansys2_support_py/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | plansys2_support_py 5 | 3.0.0 6 | 7 | This package contains modules for developing PlanSys components in Python 8 | 9 | Francisco Martin Rico 10 | 11 | Apache License, Version 2.0 12 | 13 | ament_cmake 14 | python_cmake_module 15 | 16 | rclpy 17 | lifecycle_msgs 18 | plansys2_msgs 19 | 20 | ament_copyright 21 | ament_flake8 22 | ament_pep257 23 | python3-pytest 24 | 25 | 26 | ament_cmake 27 | 28 | 29 | -------------------------------------------------------------------------------- /plansys2_support_py/plansys2_support_py/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright 2023 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 | -------------------------------------------------------------------------------- /plansys2_terminal/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | project(plansys2_terminal) 4 | 5 | find_package(ament_cmake REQUIRED) 6 | find_package(rclcpp REQUIRED) 7 | find_package(plansys2_msgs REQUIRED) 8 | find_package(plansys2_domain_expert REQUIRED) 9 | find_package(plansys2_problem_expert REQUIRED) 10 | find_package(plansys2_planner REQUIRED) 11 | find_package(plansys2_executor REQUIRED) 12 | find_package(plansys2_pddl_parser REQUIRED) 13 | 14 | add_library(terminal SHARED 15 | src/plansys2_terminal/Terminal.cpp 16 | ) 17 | target_include_directories(terminal PUBLIC 18 | "$" 19 | "$" 20 | ) 21 | target_link_libraries(terminal 22 | PUBLIC 23 | plansys2_domain_expert::plansys2_domain_expert 24 | plansys2_problem_expert::plansys2_problem_expert 25 | plansys2_planner::plansys2_planner 26 | plansys2_executor::plansys2_executor 27 | rclcpp::rclcpp 28 | PRIVATE 29 | plansys2_pddl_parser::plansys2_pddl_parser 30 | ${plansys2_msgs_TARGETS} 31 | readline 32 | ) 33 | target_compile_definitions(terminal PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") 34 | 35 | add_executable(${PROJECT_NAME} 36 | src/terminal_node.cpp) 37 | target_include_directories(${PROJECT_NAME} PRIVATE 38 | "$" 39 | "$" 40 | ) 41 | target_link_libraries(${PROJECT_NAME} 42 | PRIVATE 43 | rclcpp::rclcpp 44 | terminal 45 | ) 46 | 47 | install(TARGETS terminal 48 | ARCHIVE DESTINATION lib 49 | LIBRARY DESTINATION lib 50 | RUNTIME DESTINATION bin 51 | ) 52 | 53 | install(TARGETS ${PROJECT_NAME} 54 | RUNTIME DESTINATION lib/${PROJECT_NAME} 55 | ) 56 | 57 | if(BUILD_TESTING) 58 | find_package(ament_lint_auto REQUIRED) 59 | ament_lint_auto_find_test_dependencies() 60 | 61 | find_package(ament_cmake_gtest REQUIRED) 62 | find_package(ament_index_cpp REQUIRED) 63 | add_subdirectory(test) 64 | endif() 65 | 66 | ament_package() 67 | -------------------------------------------------------------------------------- /plansys2_terminal/README.md: -------------------------------------------------------------------------------- 1 | # Terminal 2 | 3 | The Plansys2 Terminal module is a tool to directly monitor and edit your application. See the tutorial [here](https://github.com/IntelligentRoboticsLabs/ros2_planning_system/blob/master/plansys2_docs/tutorials/tut_1_terminal.md). 4 | 5 | The node is created inside a Terminal object, which starts a looping run_console() method reading the terminal input and calling the corresponding services and actions. 6 | 7 | To enable for using parameters specified in a YAML file for e.g. specifying the timeout for the planner and planner client, you can use the following command: 8 | ``ros2 run plansys2_terminal plansys2_terminal --ros-args --params-file `ros2 pkg prefix --share plansys2_bringup`/params/plansys2_params.yaml`` 9 | 10 | -------------------------------------------------------------------------------- /plansys2_terminal/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | plansys2_terminal 5 | 3.0.0 6 | 7 | A terminal tool for monitor and manage the ROS2 Planning System 8 | 9 | Francisco Martin Rico 10 | 11 | Apache License, Version 2.0 12 | 13 | ament_cmake 14 | 15 | rclcpp 16 | plansys2_msgs 17 | plansys2_domain_expert 18 | plansys2_problem_expert 19 | plansys2_planner 20 | plansys2_executor 21 | plansys2_pddl_parser 22 | libreadline 23 | 24 | ament_lint_common 25 | ament_lint_auto 26 | ament_cmake_gtest 27 | ament_index_cpp 28 | 29 | 30 | ament_cmake 31 | 32 | 33 | -------------------------------------------------------------------------------- /plansys2_terminal/src/terminal_node.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 | 16 | #include 17 | 18 | #include "rclcpp/rclcpp.hpp" 19 | 20 | #include "plansys2_terminal/Terminal.hpp" 21 | 22 | int main(int argc, char ** argv) 23 | { 24 | rclcpp::init(argc, argv); 25 | auto terminal_node = std::make_shared(); 26 | 27 | terminal_node->run_console(); 28 | 29 | rclcpp::shutdown(); 30 | return 0; 31 | } 32 | -------------------------------------------------------------------------------- /plansys2_terminal/test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ament_add_gtest(terminal_test terminal_test.cpp TIMEOUT 500) 2 | target_link_libraries(terminal_test 3 | terminal 4 | readline 5 | ament_index_cpp::ament_index_cpp 6 | ) 7 | 8 | install(DIRECTORY 9 | pddl 10 | DESTINATION share/${PROJECT_NAME} 11 | ) 12 | -------------------------------------------------------------------------------- /plansys2_terminal/test/pddl/commands: -------------------------------------------------------------------------------- 1 | set instance leia robot 2 | set instance entrance room 3 | set instance kitchen room 4 | set instance bedroom room 5 | set instance dinning room 6 | set instance bathroom room 7 | set instance chargingroom room 8 | 9 | set predicate (connected entrance dinning) 10 | set predicate (connected dinning entrance) 11 | set predicate (connected dinning kitchen) 12 | set predicate (connected kitchen dinning) 13 | set predicate (connected dinning bedroom) 14 | set predicate (connected bedroom dinning) 15 | set predicate (connected bathroom bedroom) 16 | set predicate (connected bedroom bathroom) 17 | set predicate (connected chargingroom kitchen) 18 | set predicate (connected kitchen chargingroom) 19 | set predicate (charging_point_at chargingroom) 20 | set predicate (battery_low leia) 21 | set predicate (robot_at leia entrance) 22 | 23 | set goal (and(robot_at leia bathroom)) 24 | 25 | quit 26 | 27 | get plan 28 | 29 | run 30 | 31 | get plan 32 | 33 | run (move leia entrance dinning) 34 | run (askcharge leia entrance) 35 | 36 | run (move leia bedroom kitchen) 37 | 38 | remove predicate battery_full leia 39 | set predicate battery_low leia 40 | -------------------------------------------------------------------------------- /plansys2_terminal/test/pddl/plan: -------------------------------------------------------------------------------- 1 | 0.000: (askcharge leia entrance chargingroom) [5.000] 2 | 5.001: (charge leia chargingroom) [5.000] 3 | 10.002: (move leia chargingroom kitchen) [5.000] 4 | 15.003: (move leia kitchen dinning) [5.000] 5 | 20.004: (move leia dinning bedroom) [5.000] 6 | 25.005: (move leia bedroom bathroom) [5.000] 7 | -------------------------------------------------------------------------------- /plansys2_terminal/test/pddl/problem_charging.pddl: -------------------------------------------------------------------------------- 1 | (define (problem simple-problem) 2 | (:domain simple) 3 | 4 | ;; Instantiate the objects. 5 | (:objects 6 | r2d2 - robot 7 | rm0 rm1 rm2 - room 8 | ) 9 | 10 | (:init 11 | ;; Define the initial state predicates. 12 | (robot_at r2d2 rm0) 13 | (charging_point_at rm0) 14 | 15 | (connected rm0 rm1) 16 | (connected rm1 rm0) 17 | (connected rm1 rm2) 18 | (connected rm2 rm1) 19 | (battery_low r2d2) 20 | ) 21 | 22 | (:goal (and 23 | (robot_at r2d2 rm2) 24 | )) 25 | 26 | (:metric 27 | minimize (total-time) 28 | ) 29 | ) 30 | -------------------------------------------------------------------------------- /plansys2_terminal/test/pddl/problem_empty_domain.pddl: -------------------------------------------------------------------------------- 1 | (define (problem simple_1) 2 | (:domain ) 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_terminal/test/pddl/simple_example.pddl: -------------------------------------------------------------------------------- 1 | (define (domain simple) 2 | (:requirements :strips :typing :adl :fluents :durative-actions) 3 | 4 | ;; Types ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; 5 | (:types 6 | robot 7 | room 8 | );; end Types ;;;;;;;;;;;;;;;;;;;;;;;;; 9 | 10 | ;; Predicates ;;;;;;;;;;;;;;;;;;;;;;;;; 11 | (:predicates 12 | 13 | (robot_at ?r - robot ?ro - room) 14 | (connected ?ro1 ?ro2 - room) 15 | (battery_full ?r - robot) 16 | (battery_low ?r - robot) 17 | (charging_point_at ?ro - room) 18 | 19 | );; end Predicates ;;;;;;;;;;;;;;;;;;;; 20 | ;; Functions ;;;;;;;;;;;;;;;;;;;;;;;;; 21 | (:functions 22 | 23 | );; end Functions ;;;;;;;;;;;;;;;;;;;; 24 | ;; Actions ;;;;;;;;;;;;;;;;;;;;;;;;;;;; 25 | (:durative-action move 26 | :parameters (?r - robot ?r1 ?r2 - room) 27 | :duration ( = ?duration 5) 28 | :condition (and 29 | (at start(connected ?r1 ?r2)) 30 | (at start(robot_at ?r ?r1)) 31 | (over all(battery_full ?r)) 32 | ) 33 | :effect (and 34 | (at start(not(robot_at ?r ?r1))) 35 | (at end(robot_at ?r ?r2)) 36 | ) 37 | ) 38 | 39 | (:durative-action askcharge 40 | :parameters (?r - robot ?r1 ?r2 - room) 41 | :duration ( = ?duration 5) 42 | :condition (and 43 | (at start(robot_at ?r ?r1)) 44 | (at start(charging_point_at ?r2)) 45 | ) 46 | :effect (and 47 | (at start(not(robot_at ?r ?r1))) 48 | (at start(robot_at ?r ?r2)) 49 | ) 50 | ) 51 | 52 | (:durative-action charge 53 | :parameters (?r - robot ?ro - room) 54 | :duration ( = ?duration 5) 55 | :condition (and 56 | (at start(robot_at ?r ?ro)) 57 | (at start(charging_point_at ?ro)) 58 | ) 59 | :effect (and 60 | (at end(not(battery_low ?r))) 61 | (at end(battery_full ?r)) 62 | ) 63 | ) 64 | 65 | );; end Domain ;;;;;;;;;;;;;;;;;;;;;;;; 66 | -------------------------------------------------------------------------------- /plansys2_tests/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | project(plansys2_tests) 4 | 5 | find_package(ament_cmake REQUIRED) 6 | find_package(plansys2_executor REQUIRED) 7 | find_package(plansys2_msgs REQUIRED) 8 | find_package(rclcpp REQUIRED) 9 | 10 | set(${PROJECT_NAME}_SOURCES 11 | src/plansys2_tests/test_action_node.cpp 12 | src/plansys2_tests/execution_logger.cpp 13 | ) 14 | 15 | add_library(${PROJECT_NAME} SHARED ${${PROJECT_NAME}_SOURCES}) 16 | target_include_directories(${PROJECT_NAME} PUBLIC 17 | "$" 18 | "$" 19 | ) 20 | target_link_libraries(${PROJECT_NAME} 21 | PUBLIC 22 | ${plansys2_msgs_TARGETS} 23 | plansys2_executor::plansys2_executor 24 | rclcpp::rclcpp 25 | ) 26 | 27 | if(BUILD_TESTING) 28 | find_package(ament_lint_auto REQUIRED) 29 | ament_lint_auto_find_test_dependencies() 30 | 31 | find_package(ament_cmake_gtest REQUIRED) 32 | find_package(ament_index_cpp REQUIRED) 33 | find_package(plansys2_domain_expert REQUIRED) 34 | find_package(plansys2_problem_expert REQUIRED) 35 | find_package(plansys2_planner REQUIRED) 36 | find_package(plansys2_pddl_parser REQUIRED) 37 | 38 | add_subdirectory(test_1) 39 | add_subdirectory(test_2) 40 | add_subdirectory(test_3) 41 | add_subdirectory(test_4) 42 | endif() 43 | 44 | ament_package() 45 | -------------------------------------------------------------------------------- /plansys2_tests/README.md: -------------------------------------------------------------------------------- 1 | # PlanSys2 tests 2 | 3 | -------------------------------------------------------------------------------- /plansys2_tests/include/plansys2_tests/test_action_node.hpp: -------------------------------------------------------------------------------- 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 | #ifndef PLANSYS2_TESTS__TEST_ACTION_NODE_HPP_ 16 | #define PLANSYS2_TESTS__TEST_ACTION_NODE_HPP_ 17 | 18 | #include 19 | #include 20 | 21 | #include "plansys2_executor/ActionExecutorClient.hpp" 22 | 23 | #include "rclcpp/rclcpp.hpp" 24 | 25 | 26 | namespace plansys2_tests 27 | { 28 | 29 | using namespace std::chrono_literals; // NOLINT (build/namespaces) 30 | 31 | class TestAction : public plansys2::ActionExecutorClient 32 | { 33 | public: 34 | using Ptr = std::shared_ptr; 35 | static Ptr make_shared( 36 | const std::string & action, float increment = 0.4) 37 | { 38 | auto ret = std::make_shared(action, increment); 39 | ret->set_parameter(rclcpp::Parameter("action_name", action)); 40 | ret->set_parameter(rclcpp::Parameter("rate", 1.0)); 41 | ret->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); 42 | return ret; 43 | } 44 | 45 | explicit TestAction(const std::string & action_name, float increment = 0.4); 46 | 47 | private: 48 | void do_work(); 49 | 50 | static int node_name_counter_; 51 | float progress_; 52 | float increment_; 53 | }; 54 | 55 | } // namespace plansys2_tests 56 | 57 | #endif // PLANSYS2_TESTS__TEST_ACTION_NODE_HPP_ 58 | -------------------------------------------------------------------------------- /plansys2_tests/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | plansys2_tests 5 | 3.0.0 6 | 7 | This package contains the tests package for the ROS2 Planning System 8 | 9 | Francisco Martin Rico 10 | 11 | Apache License, Version 2.0 12 | 13 | ament_cmake 14 | 15 | rclcpp 16 | plansys2_msgs 17 | plansys2_executor 18 | 19 | popf 20 | 21 | ament_lint_common 22 | ament_lint_auto 23 | ament_cmake_gtest 24 | ament_index_cpp 25 | plansys2_domain_expert 26 | plansys2_problem_expert 27 | plansys2_planner 28 | plansys2_pddl_parser 29 | 30 | 31 | ament_cmake 32 | 33 | 34 | -------------------------------------------------------------------------------- /plansys2_tests/src/plansys2_tests/test_action_node.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 | 17 | #include "plansys2_tests/test_action_node.hpp" 18 | 19 | #include "plansys2_executor/ActionExecutorClient.hpp" 20 | 21 | #include "rclcpp/rclcpp.hpp" 22 | 23 | 24 | namespace plansys2_tests 25 | { 26 | 27 | int TestAction::node_name_counter_ = 0; 28 | 29 | TestAction::TestAction(const std::string & action_name, float increment) 30 | : plansys2::ActionExecutorClient(action_name + std::to_string(node_name_counter_++)), 31 | increment_(increment) 32 | { 33 | progress_ = 0.0; 34 | } 35 | 36 | void 37 | TestAction::do_work() 38 | { 39 | if (progress_ < 1.0) { 40 | progress_ += increment_; 41 | send_feedback(progress_, "Action running"); 42 | } else { 43 | finish(true, 1.0, "Action completed"); 44 | progress_ = 0.0; 45 | } 46 | } 47 | 48 | } // namespace plansys2_tests 49 | -------------------------------------------------------------------------------- /plansys2_tests/test_1/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | install(DIRECTORY pddl 2 | DESTINATION share/${PROJECT_NAME}/test_1 3 | ) 4 | 5 | ament_add_gtest(test_1 test_1.cpp) 6 | target_link_libraries(test_1 7 | ${PROJECT_NAME} 8 | ament_index_cpp::ament_index_cpp 9 | plansys2_domain_expert::plansys2_domain_expert 10 | plansys2_problem_expert::plansys2_problem_expert 11 | plansys2_planner::plansys2_planner 12 | ) 13 | -------------------------------------------------------------------------------- /plansys2_tests/test_1/pddl/test_1.pddl: -------------------------------------------------------------------------------- 1 | (define (domain simple) 2 | (:requirements :strips :typing :adl :fluents :durative-actions) 3 | 4 | ;; Types ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; 5 | (:types 6 | robot 7 | room 8 | );; end Types ;;;;;;;;;;;;;;;;;;;;;;;;; 9 | 10 | ;; Predicates ;;;;;;;;;;;;;;;;;;;;;;;;; 11 | (:predicates 12 | 13 | (robot_at ?r - robot ?ro - room) 14 | (connected ?ro1 ?ro2 - room) 15 | (battery_full ?r - robot) 16 | (battery_low ?r - robot) 17 | (charging_point_at ?ro - room) 18 | 19 | );; end Predicates ;;;;;;;;;;;;;;;;;;;; 20 | ;; Functions ;;;;;;;;;;;;;;;;;;;;;;;;; 21 | (:functions 22 | 23 | );; end Functions ;;;;;;;;;;;;;;;;;;;; 24 | ;; Actions ;;;;;;;;;;;;;;;;;;;;;;;;;;;; 25 | (:durative-action move 26 | :parameters (?r - robot ?r1 ?r2 - room) 27 | :duration ( = ?duration 5) 28 | :condition (and 29 | (at start(connected ?r1 ?r2)) 30 | (at start(robot_at ?r ?r1)) 31 | (over all(battery_full ?r)) 32 | ) 33 | :effect (and 34 | (at start(not(robot_at ?r ?r1))) 35 | (at end(robot_at ?r ?r2)) 36 | ) 37 | ) 38 | 39 | (:durative-action askcharge 40 | :parameters (?r - robot ?r1 ?r2 - room) 41 | :duration ( = ?duration 5) 42 | :condition (and 43 | (at start(robot_at ?r ?r1)) 44 | (at start(charging_point_at ?r2)) 45 | ) 46 | :effect (and 47 | (at start(not(robot_at ?r ?r1))) 48 | (at start(robot_at ?r ?r2)) 49 | ) 50 | ) 51 | 52 | (:durative-action charge 53 | :parameters (?r - robot ?ro - room) 54 | :duration ( = ?duration 5) 55 | :condition (and 56 | (at start(robot_at ?r ?ro)) 57 | (at start(charging_point_at ?ro)) 58 | ) 59 | :effect (and 60 | (at end(not(battery_low ?r))) 61 | (at end(battery_full ?r)) 62 | ) 63 | ) 64 | 65 | );; end Domain ;;;;;;;;;;;;;;;;;;;;;;;; 66 | -------------------------------------------------------------------------------- /plansys2_tests/test_2/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | install(DIRECTORY pddl 2 | DESTINATION share/${PROJECT_NAME}/test_2 3 | ) 4 | 5 | ament_add_gtest(test_2 test_2.cpp) 6 | target_link_libraries(test_2 7 | ${PROJECT_NAME} 8 | plansys2_domain_expert::plansys2_domain_expert 9 | plansys2_problem_expert::plansys2_problem_expert 10 | plansys2_planner::plansys2_planner 11 | ) 12 | -------------------------------------------------------------------------------- /plansys2_tests/test_2/pddl/problem_2.pddl: -------------------------------------------------------------------------------- 1 | ( define ( problem problem_1 ) 2 | ( :domain test2 ) 3 | ( :objects 4 | leia - robot 5 | room_1 room_2 - room 6 | corridor_1 - corridor 7 | zone_1_1 zone_1_2 zone_recharge - zone 8 | ) 9 | ( :init 10 | ( connected room_1 corridor_1 ) 11 | ( connected corridor_1 room_1 ) 12 | ( connected room_2 corridor_1 ) 13 | ( connected corridor_1 room_2 ) 14 | ( connected room_1 zone_1_1 ) 15 | ( connected zone_1_1 room_1 ) 16 | ( connected room_1 zone_1_2 ) 17 | ( connected zone_1_2 room_1 ) 18 | ( connected room_2 zone_recharge ) 19 | ( connected zone_recharge room_2 ) 20 | ( charging_point_at zone_recharge ) 21 | ( battery_low leia ) 22 | ( robot_at leia zone_1_1 ) 23 | ) 24 | ( :goal 25 | ( and 26 | ( robot_at leia zone_1_2 ) 27 | ) 28 | ) 29 | ) -------------------------------------------------------------------------------- /plansys2_tests/test_2/pddl/test_2.pddl: -------------------------------------------------------------------------------- 1 | (define (domain test2) 2 | (:requirements :strips :typing :adl :fluents :durative-actions) 3 | 4 | ;; Types ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; 5 | (:types 6 | robot 7 | room zone corridor - place 8 | );; end Types ;;;;;;;;;;;;;;;;;;;;;;;;; 9 | 10 | ;; Predicates ;;;;;;;;;;;;;;;;;;;;;;;;; 11 | (:predicates 12 | 13 | (robot_at_room ?r - robot ?ro - room) 14 | (robot_at_zone ?r - robot ?z - zone) 15 | (robot_at_corridor ?r - robot ?c - corridor) 16 | (robot_at ?r - robot ?p - place) 17 | (connected ?p1 ?p2 - place) 18 | (battery_full ?r - robot) 19 | (battery_low ?r - robot) 20 | (charging_point_at ?z - zone) 21 | 22 | );; end Predicates ;;;;;;;;;;;;;;;;;;;; 23 | ;; Functions ;;;;;;;;;;;;;;;;;;;;;;;;; 24 | (:functions 25 | 26 | );; end Functions ;;;;;;;;;;;;;;;;;;;; 27 | ;; Actions ;;;;;;;;;;;;;;;;;;;;;;;;;;;; 28 | (:durative-action move 29 | :parameters (?r - robot ?p1 ?p2 - place) 30 | :duration ( = ?duration 5) 31 | :condition (and 32 | (at start(connected ?p1 ?p2)) 33 | (at start(robot_at ?r ?p1)) 34 | (over all(battery_full ?r)) 35 | ) 36 | :effect (and 37 | (at start(not(robot_at ?r ?p1))) 38 | (at end(robot_at ?r ?p2)) 39 | ) 40 | ) 41 | 42 | (:durative-action askcharge 43 | :parameters (?r - robot ?p1 - place ?p2 - zone) 44 | :duration ( = ?duration 5) 45 | :condition (and 46 | (at start(robot_at ?r ?p1)) 47 | (at start(charging_point_at ?p2)) 48 | ) 49 | :effect (and 50 | (at start(not(robot_at ?r ?p1))) 51 | (at start(robot_at ?r ?p2)) 52 | ) 53 | ) 54 | 55 | (:durative-action charge 56 | :parameters (?r - robot ?z - zone) 57 | :duration ( = ?duration 5) 58 | :condition (and 59 | (at start(robot_at ?r ?z)) 60 | (at start(charging_point_at ?z)) 61 | ) 62 | :effect (and 63 | (at end(not(battery_low ?r))) 64 | (at end(battery_full ?r)) 65 | ) 66 | ) 67 | 68 | );; end Domain ;;;;;;;;;;;;;;;;;;;;;;;; 69 | -------------------------------------------------------------------------------- /plansys2_tests/test_3/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | install(DIRECTORY pddl 2 | DESTINATION share/${PROJECT_NAME}/test_3 3 | ) 4 | 5 | ament_add_gtest(test_3 test_3.cpp TIMEOUT 300) 6 | target_link_libraries(test_3 7 | ${PROJECT_NAME} 8 | ament_index_cpp::ament_index_cpp 9 | plansys2_domain_expert::plansys2_domain_expert 10 | plansys2_problem_expert::plansys2_problem_expert 11 | plansys2_planner::plansys2_planner 12 | plansys2_pddl_parser::plansys2_pddl_parser 13 | ) 14 | -------------------------------------------------------------------------------- /plansys2_tests/test_4/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | install(DIRECTORY pddl 2 | DESTINATION share/${PROJECT_NAME}/test_4 3 | ) 4 | 5 | ament_add_gtest(test_4 test_4.cpp) 6 | target_link_libraries(test_4 ${PROJECT_NAME}) 7 | -------------------------------------------------------------------------------- /plansys2_tests/test_4/README.md: -------------------------------------------------------------------------------- 1 | # Test 4 2 | 3 | Short lineal plan with 1 action. 4 | 5 | ## PlanSys2 6 | 7 | - [x] Regular PlanSys2 actions 8 | - [ ] Regular PlanSys2 actions with ROS2 action client 9 | - [ ] PlanSys2 BT actions 10 | - [ ] PlanSys2 BT actions with ROS2 action client 11 | 12 | ## PDDL 13 | 14 | - [ ] Types 15 | - [x] Predicates 16 | - [ ] Durative actions 17 | - [x] at start req 18 | - [ ] over all req 19 | - [ ] at end req 20 | - [ ] at start effect 21 | - [x] at end effect 22 | 23 | ### Domain 24 | 25 | ``` 26 | (define (domain simple-door) 27 | (:requirements :strips :typing :adl :fluents :durative-actions :existential-preconditions) 28 | 29 | (:predicates 30 | (door_open) 31 | (not_door_open) 32 | ) 33 | 34 | (:durative-action open-door 35 | :parameters () 36 | :duration ( = ?duration 1) 37 | :condition (and 38 | (at start (not_door_open)) 39 | ) 40 | :effect (and 41 | (at end (door_open)) 42 | ) 43 | ) 44 | 45 | (:durative-action close-door 46 | :parameters () 47 | :duration ( = ?duration 1) 48 | :condition (and 49 | (at start (door_open)) 50 | ) 51 | :effect (and 52 | (at end (not_door_open)) 53 | ) 54 | ) 55 | ) 56 | 57 | ``` 58 | 59 | ### Problem 60 | 61 | ``` 62 | (define (problem open-the-door) 63 | (:domain simple-door) 64 | (:init 65 | (not_door_open) 66 | ) 67 | (:goal 68 | (and (door_open)) 69 | ) 70 | ) 71 | ``` 72 | 73 | ### Plan 74 | 75 | ``` 76 | 0.000: (open-door) [1.000] 77 | ``` 78 | 79 | -------------------------------------------------------------------------------- /plansys2_tests/test_4/pddl/test_4.pddl: -------------------------------------------------------------------------------- 1 | (define (domain simple-door) 2 | (:requirements :strips :typing :adl :fluents :durative-actions :existential-preconditions) 3 | 4 | (:predicates 5 | (door_open) 6 | (not_door_open) 7 | ) 8 | 9 | (:durative-action open_door 10 | :parameters () 11 | :duration ( = ?duration 1) 12 | :condition (and 13 | (at start (not_door_open)) 14 | ) 15 | :effect (and 16 | (at end (door_open)) 17 | (at end (not (not_door_open))) 18 | ) 19 | ) 20 | 21 | (:durative-action close_door 22 | :parameters () 23 | :duration ( = ?duration 1) 24 | :condition (and 25 | (at start (door_open)) 26 | ) 27 | :effect (and 28 | (at end (not_door_open)) 29 | (at end (not (door_open))) 30 | ) 31 | ) 32 | ) 33 | -------------------------------------------------------------------------------- /plansys2_tools/README.md: -------------------------------------------------------------------------------- 1 | # Tools 2 | -------------------------------------------------------------------------------- /plansys2_tools/include/rqt_plansys2_knowledge/KnowledgeTree.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 RQT_PLANSYS2_KNOWLEDGE__KNOWLEDGETREE_HPP_ 16 | #define RQT_PLANSYS2_KNOWLEDGE__KNOWLEDGETREE_HPP_ 17 | 18 | #include 19 | #include 20 | 21 | namespace rqt_plansys2_knowledge 22 | { 23 | 24 | class KnowledgeTree : public QTreeWidget 25 | { 26 | public: 27 | KnowledgeTree(); 28 | 29 | void clearAllItems(); 30 | }; 31 | 32 | } // namespace rqt_plansys2_knowledge 33 | 34 | #endif // RQT_PLANSYS2_KNOWLEDGE__KNOWLEDGETREE_HPP_ 35 | -------------------------------------------------------------------------------- /plansys2_tools/include/rqt_plansys2_performers/PerformersTree.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 RQT_PLANSYS2_PERFORMERS__PERFORMERSTREE_HPP_ 16 | #define RQT_PLANSYS2_PERFORMERS__PERFORMERSTREE_HPP_ 17 | 18 | #include 19 | #include 20 | 21 | namespace rqt_plansys2_performers 22 | { 23 | 24 | class PerformersTree : public QTreeWidget 25 | { 26 | public: 27 | PerformersTree(); 28 | 29 | void clearAllItems(); 30 | }; 31 | 32 | } // namespace rqt_plansys2_performers 33 | 34 | #endif // RQT_PLANSYS2_PERFORMERS__PERFORMERSTREE_HPP_ 35 | -------------------------------------------------------------------------------- /plansys2_tools/include/rqt_plansys2_plan/PlanTree.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 RQT_PLANSYS2_PLAN__PLANTREE_HPP_ 16 | #define RQT_PLANSYS2_PLAN__PLANTREE_HPP_ 17 | 18 | #include 19 | #include 20 | 21 | namespace rqt_plansys2_plan 22 | { 23 | 24 | class PlanTree : public QTreeWidget 25 | { 26 | public: 27 | PlanTree(); 28 | 29 | void clearAllItems(); 30 | }; 31 | 32 | } // namespace rqt_plansys2_plan 33 | 34 | #endif // RQT_PLANSYS2_PLAN__PLANTREE_HPP_ 35 | -------------------------------------------------------------------------------- /plansys2_tools/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | plansys2_tools 5 | 3.0.0 6 | 7 | A set of tools for monitoring ROS2 Planning System 8 | 9 | Francisco Martin Rico 10 | 11 | Apache License, Version 2.0 12 | 13 | ament_cmake 14 | 15 | qtbase5-dev 16 | 17 | rclcpp 18 | rclcpp_lifecycle 19 | plansys2_msgs 20 | rqt_gui 21 | rqt_gui_cpp 22 | qt_gui_cpp 23 | plansys2_problem_expert 24 | 25 | libqt5-widgets 26 | 27 | ament_lint_common 28 | ament_lint_auto 29 | ament_cmake_gtest 30 | ament_index_cpp 31 | 32 | 33 | ament_cmake 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /plansys2_tools/plugin_knowledge.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | A GUI plugin to view PlanSys2 Knowledge 5 | 6 | 7 | 8 | 9 | applications-accesories 10 | PlanSys2 Plugins. 11 | 12 | 13 | utilities-system-monitor 14 | A GUI plugin for viewing Knowledge. 15 | 16 | 17 | -------------------------------------------------------------------------------- /plansys2_tools/plugin_performers.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | A GUI plugin to view PlanSys2 Performers 5 | 6 | 7 | 8 | 9 | applications-accesories 10 | PlanSys2 Plugins. 11 | 12 | 13 | utilities-system-monitor 14 | A GUI plugin for viewing Performers. 15 | 16 | 17 | -------------------------------------------------------------------------------- /plansys2_tools/plugin_plan.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | A GUI plugin to view PlanSys2 Plan 5 | 6 | 7 | 8 | 9 | applications-accesories 10 | PlanSys2 Plugins. 11 | 12 | 13 | utilities-system-monitor 14 | A GUI plugin for viewing Plan. 15 | 16 | 17 | -------------------------------------------------------------------------------- /plansys2_tools/scripts/rqt_plansys2_knowledge: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import sys 4 | 5 | from rqt_gui.main import Main 6 | 7 | 8 | def add_arguments(parser): 9 | group = parser.add_argument_group('Options for rqt_plansys2_knowledge plugin') 10 | group.add_argument('topic', nargs='?', help='The topic name to subscribe to') 11 | 12 | main = Main() 13 | sys.exit(main.main( 14 | sys.argv, 15 | standalone='rqt_plansys2_knowledge/RQTKnowledge', 16 | plugin_argument_provider=add_arguments)) 17 | -------------------------------------------------------------------------------- /plansys2_tools/scripts/rqt_plansys2_performers: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import sys 4 | 5 | from rqt_gui.main import Main 6 | 7 | 8 | def add_arguments(parser): 9 | group = parser.add_argument_group('Options for rqt_plansys2_performers plugin') 10 | group.add_argument('topic', nargs='?', help='The topic name to subscribe to') 11 | 12 | main = Main() 13 | sys.exit(main.main( 14 | sys.argv, 15 | standalone='rqt_plansys2_performers/RQTPerformers', 16 | plugin_argument_provider=add_arguments)) 17 | -------------------------------------------------------------------------------- /plansys2_tools/scripts/rqt_plansys2_plan: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import sys 4 | 5 | from rqt_gui.main import Main 6 | 7 | 8 | def add_arguments(parser): 9 | group = parser.add_argument_group('Options for rqt_plansys2_plan plugin') 10 | group.add_argument('topic', nargs='?', help='The topic name to subscribe to') 11 | 12 | main = Main() 13 | sys.exit(main.main( 14 | sys.argv, 15 | standalone='rqt_plansys2_plan/RQTPlan', 16 | plugin_argument_provider=add_arguments)) 17 | -------------------------------------------------------------------------------- /plansys2_tools/setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from distutils.core import setup 4 | 5 | from catkin_pkg.python_setup import generate_distutils_setup 6 | 7 | d = generate_distutils_setup( 8 | packages=['plansys2_tools'], 9 | package_dir={'': 'src'}, 10 | scripts=['scripts/rqt_plansys2_knowledge'] 11 | ) 12 | 13 | setup(**d) 14 | 15 | d2 = generate_distutils_setup( 16 | packages=['plansys2_tools'], 17 | package_dir={'': 'src'}, 18 | scripts=['scripts/rqt_plansys2_performers'] 19 | ) 20 | 21 | setup(**d2) 22 | 23 | d3 = generate_distutils_setup( 24 | packages=['plansys2_tools'], 25 | package_dir={'': 'src'}, 26 | scripts=['scripts/rqt_plansys2_plan'] 27 | ) 28 | 29 | setup(**d2) 30 | -------------------------------------------------------------------------------- /plansys2_tools/src/logger.cpp: -------------------------------------------------------------------------------- 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 | #include 17 | 18 | #include "rclcpp/rclcpp.hpp" 19 | 20 | #include "plansys2_logger/LoggerNode.hpp" 21 | 22 | int main(int argc, char ** argv) 23 | { 24 | rclcpp::init(argc, argv); 25 | auto logger_node = std::make_shared(); 26 | 27 | rclcpp::spin(logger_node); 28 | 29 | rclcpp::shutdown(); 30 | return 0; 31 | } 32 | -------------------------------------------------------------------------------- /plansys2_tools/src/rqt_plansys2_knowledge/KnowledgeTree.cpp: -------------------------------------------------------------------------------- 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 | #include 17 | #include 18 | 19 | #include "rqt_plansys2_knowledge/KnowledgeTree.hpp" 20 | 21 | namespace rqt_plansys2_knowledge 22 | { 23 | 24 | KnowledgeTree::KnowledgeTree() 25 | : QTreeWidget() 26 | { 27 | } 28 | 29 | void 30 | KnowledgeTree::clearAllItems() 31 | { 32 | int rows = topLevelItemCount(); 33 | 34 | while (topLevelItemCount() > 0) { 35 | rowsAboutToBeRemoved(rootIndex(), 0, 0); 36 | delete takeTopLevelItem(0); 37 | rowsRemoved(rootIndex(), 0, 0);/* code */ 38 | } 39 | } 40 | 41 | } // namespace rqt_plansys2_knowledge 42 | -------------------------------------------------------------------------------- /plansys2_tools/src/rqt_plansys2_knowledge/rqt_plansys2_knowledge.ui: -------------------------------------------------------------------------------- 1 | 2 | 3 | RqtPlansys2Knowledge 4 | 5 | 6 | RqtPlansys2Knowledge 7 | 8 | 9 | 10 | 11 | 0 12 | 10 13 | 791 14 | 431 15 | 16 | 17 | 18 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /plansys2_tools/src/rqt_plansys2_performers/PerformersTree.cpp: -------------------------------------------------------------------------------- 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 | #include 17 | #include 18 | 19 | #include "rqt_plansys2_performers/PerformersTree.hpp" 20 | 21 | namespace rqt_plansys2_performers 22 | { 23 | 24 | PerformersTree::PerformersTree() 25 | : QTreeWidget() 26 | { 27 | } 28 | 29 | void 30 | PerformersTree::clearAllItems() 31 | { 32 | int rows = topLevelItemCount(); 33 | 34 | while (topLevelItemCount() > 0) { 35 | rowsAboutToBeRemoved(rootIndex(), 0, 0); 36 | delete takeTopLevelItem(0); 37 | rowsRemoved(rootIndex(), 0, 0);/* code */ 38 | } 39 | } 40 | 41 | } // namespace rqt_plansys2_performers 42 | -------------------------------------------------------------------------------- /plansys2_tools/src/rqt_plansys2_performers/rqt_plansys2_performers.ui: -------------------------------------------------------------------------------- 1 | 2 | 3 | RqtPlansys2Performers 4 | 5 | 6 | RqtPlansys2Performers 7 | 8 | 9 | 10 | 11 | 0 12 | 10 13 | 791 14 | 431 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /plansys2_tools/src/rqt_plansys2_plan/PlanTree.cpp: -------------------------------------------------------------------------------- 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 | #include 17 | #include 18 | 19 | #include "rqt_plansys2_plan/PlanTree.hpp" 20 | 21 | namespace rqt_plansys2_plan 22 | { 23 | 24 | PlanTree::PlanTree() 25 | : QTreeWidget() 26 | { 27 | } 28 | 29 | void 30 | PlanTree::clearAllItems() 31 | { 32 | int rows = topLevelItemCount(); 33 | 34 | while (topLevelItemCount() > 0) { 35 | rowsAboutToBeRemoved(rootIndex(), 0, 0); 36 | delete takeTopLevelItem(0); 37 | rowsRemoved(rootIndex(), 0, 0);/* code */ 38 | } 39 | } 40 | 41 | } // namespace rqt_plansys2_plan 42 | -------------------------------------------------------------------------------- /plansys2_tools/src/rqt_plansys2_plan/rqt_plansys2_plan.ui: -------------------------------------------------------------------------------- 1 | 2 | 3 | RqtPlansys2Plan 4 | 5 | 6 | RqtPlansys2Plan 7 | 8 | 9 | 10 | 11 | 0 12 | 10 13 | 1600 14 | 431 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | --------------------------------------------------------------------------------