From bbb994e82f95d13351eaccb3e8f3c11e964e5635 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Sat, 29 Oct 2022 11:27:08 -0400 Subject: [PATCH] add pre-commit files Signed-off-by: Paul Gesel --- .clang-format | 20 + .github/workflows/format.yaml | 28 + .github/workflows/foxy-devel.yaml | 2 +- .github/workflows/galactic-devel.yaml | 2 +- .github/workflows/humble-devel.yaml | 2 +- .pre-commit-config.yaml | 54 + README.md | 4 +- plansys2_bringup/CMakeLists.txt | 1 + .../plansys2_bringup_launch_distributed.py | 158 +- .../plansys2_bringup_launch_monolithic.py | 85 +- plansys2_bringup/package.xml | 4 +- plansys2_bringup/src/plansys2_node.cpp | 30 +- plansys2_bt_actions/CMakeLists.txt | 1 + plansys2_bt_actions/README.md | 3 +- .../include/plansys2_bt_actions/BTAction.hpp | 27 +- .../plansys2_bt_actions/BTActionNode.hpp | 126 +- plansys2_bt_actions/package.xml | 4 +- plansys2_bt_actions/src/bt_action_node.cpp | 1 - .../src/plansys2_bt_actions/BTAction.cpp | 70 +- .../test/behavior_tree/CloseGripper.cpp | 20 +- .../test/behavior_tree/CloseGripper.hpp | 16 +- .../test/behavior_tree/FailureNodes.cpp | 30 +- .../test/behavior_tree/FailureNodes.hpp | 27 +- .../test/behavior_tree/Move.cpp | 39 +- .../test/behavior_tree/Move.hpp | 13 +- .../test/behavior_tree/OpenGripper.cpp | 20 +- .../test/behavior_tree/OpenGripper.hpp | 10 +- .../test/unit/bt_action_test.cpp | 73 +- plansys2_core/CMakeLists.txt | 1 + .../include/plansys2_core/PlanSolverBase.hpp | 4 +- plansys2_core/include/plansys2_core/Types.hpp | 52 +- plansys2_core/include/plansys2_core/Utils.hpp | 5 +- plansys2_core/package.xml | 4 +- plansys2_core/src/plansys2_core/Utils.cpp | 11 +- plansys2_core/test/utils_test.cpp | 2 +- plansys2_docs/tutorials/tut_1_terminal.md | 2 +- plansys2_docs/tutorials/tut_2_patrol.md | 2 +- plansys2_domain_expert/CHANGELOG.rst | 2 +- plansys2_domain_expert/CMakeLists.txt | 1 + plansys2_domain_expert/README.md | 2 +- .../plansys2_domain_expert/DomainExpert.hpp | 38 +- .../DomainExpertClient.hpp | 50 +- .../DomainExpertInterface.hpp | 42 +- .../DomainExpertNode.hpp | 30 +- .../plansys2_domain_expert/DomainReader.hpp | 3 +- .../launch/domain_expert_launch.py | 33 +- plansys2_domain_expert/package.xml | 6 +- .../plansys2_domain_expert/DomainExpert.cpp | 87 +- .../DomainExpertClient.cpp | 241 ++- .../DomainExpertNode.cpp | 166 +-- .../plansys2_domain_expert/DomainReader.cpp | 51 +- plansys2_domain_expert/test/CMakeLists.txt | 2 +- .../test/pddl/domain_charging.pddl | 6 +- .../test/pddl/domain_combined_processed.pddl | 4 +- .../test/pddl/domain_simple.pddl | 4 +- .../test/pddl/domain_simple_constants.pddl | 6 +- .../domain_simple_constants_processed.pddl | 6 +- .../test/pddl/domain_simple_processed.pddl | 4 +- .../test/pddl/problem_charging.pddl | 6 +- .../test/pddl/problem_simple_1.pddl | 2 +- .../test/unit/CMakeLists.txt | 2 +- .../test/unit/domain_expert_node_test.cpp | 40 +- .../test/unit/domain_expert_test.cpp | 168 +-- .../test/unit/domain_reader_test.cpp | 100 +- .../test/unit/types_test.cpp | 67 +- plansys2_executor/CMakeLists.txt | 1 + plansys2_executor/README.md | 4 +- .../behavior_trees/plansys2_action_bt.xml | 2 +- .../plansys2_executor/ActionExecutor.hpp | 35 +- .../ActionExecutorClient.hpp | 22 +- .../include/plansys2_executor/BTBuilder.hpp | 21 +- .../include/plansys2_executor/ComputeBT.hpp | 4 +- .../plansys2_executor/ExecutorClient.hpp | 13 +- .../plansys2_executor/ExecutorNode.hpp | 37 +- .../behavior_tree/apply_atend_effect_node.hpp | 20 +- .../apply_atstart_effect_node.hpp | 20 +- .../behavior_tree/check_action_node.hpp | 17 +- .../behavior_tree/check_atend_req_node.hpp | 20 +- .../behavior_tree/check_overall_req_node.hpp | 20 +- .../behavior_tree/check_timeout_node.hpp | 20 +- .../behavior_tree/execute_action_node.hpp | 16 +- .../behavior_tree/wait_action_node.hpp | 17 +- .../behavior_tree/wait_atstart_req_node.hpp | 20 +- .../bt_builder_plugins/simple_bt_builder.hpp | 79 +- .../bt_builder_plugins/stn_bt_builder.hpp | 80 +- plansys2_executor/launch/compute_bt_launch.py | 85 +- plansys2_executor/launch/executor_launch.py | 70 +- plansys2_executor/package.xml | 4 +- .../src/plansys2_executor/ActionExecutor.cpp | 87 +- .../ActionExecutorClient.cpp | 90 +- .../src/plansys2_executor/ComputeBT.cpp | 100 +- .../src/plansys2_executor/ExecutorClient.cpp | 129 +- .../src/plansys2_executor/ExecutorNode.cpp | 191 +-- .../behavior_tree/apply_atend_effect_node.cpp | 18 +- .../apply_atstart_effect_node.cpp | 18 +- .../behavior_tree/check_action_node.cpp | 22 +- .../behavior_tree/check_atend_req_node.cpp | 24 +- .../behavior_tree/check_overall_req_node.cpp | 23 +- .../behavior_tree/check_timeout_node.cpp | 29 +- .../behavior_tree/execute_action_node.cpp | 22 +- .../behavior_tree/wait_action_node.cpp | 22 +- .../behavior_tree/wait_atstart_req_node.cpp | 29 +- .../bt_builder_plugins/simple_bt_builder.cpp | 285 ++-- .../bt_builder_plugins/stn_bt_builder.cpp | 322 ++-- .../test/pddl/cooking_domain.pddl | 10 +- .../test/pddl/domain_charging.pddl | 6 +- .../test/pddl/elevator_domain.pddl | 4 +- .../test/pddl/problem_simple_1.pddl | 2 +- .../test/pddl/problem_simple_2.pddl | 2 +- .../test/pddl/problem_simple_3.pddl | 4 +- .../test/unit/action_execution_test.cpp | 88 +- plansys2_executor/test/unit/bt_node_test.cpp | 153 +- .../test/unit/bt_node_test_charging.cpp | 122 +- .../test/unit/execution_tree_test.cpp | 94 +- plansys2_executor/test/unit/executor_test.cpp | 300 ++-- .../test/unit/simple_btbuilder_tests.cpp | 284 ++-- plansys2_lifecycle_manager/CHANGELOG.rst | 2 +- plansys2_lifecycle_manager/CMakeLists.txt | 3 +- .../lifecycle_manager.hpp | 22 +- plansys2_lifecycle_manager/package.xml | 6 +- .../src/lifecycle_manager_node.cpp | 25 +- .../lifecycle_manager.cpp | 92 +- .../test/lf_manager_test.cpp | 75 +- plansys2_msgs/CHANGELOG.rst | 2 +- plansys2_msgs/README.md | 6 +- plansys2_msgs/package.xml | 4 +- plansys2_msgs/srv/ClearProblemKnowledge.srv | 2 +- plansys2_msgs/srv/RemoveProblemGoal.srv | 2 +- .../include/plansys2_pddl_parser/Action.h | 101 +- .../include/plansys2_pddl_parser/And.h | 69 +- .../include/plansys2_pddl_parser/Basic.h | 102 +- .../include/plansys2_pddl_parser/CondIter.h | 55 +- .../include/plansys2_pddl_parser/Condition.h | 52 +- .../include/plansys2_pddl_parser/Derived.h | 62 +- .../include/plansys2_pddl_parser/Domain.h | 1295 +++++++++-------- .../include/plansys2_pddl_parser/EitherType.h | 39 +- .../include/plansys2_pddl_parser/Exists.h | 63 +- .../include/plansys2_pddl_parser/Expression.h | 480 +++--- .../include/plansys2_pddl_parser/Forall.h | 62 +- .../include/plansys2_pddl_parser/Function.h | 37 +- .../plansys2_pddl_parser/FunctionModifier.h | 111 +- .../include/plansys2_pddl_parser/Ground.h | 54 +- .../include/plansys2_pddl_parser/GroundFunc.h | 40 +- .../include/plansys2_pddl_parser/Instance.h | 533 +++---- .../include/plansys2_pddl_parser/Lifted.h | 42 +- .../include/plansys2_pddl_parser/Not.h | 66 +- .../include/plansys2_pddl_parser/Oneof.h | 70 +- .../include/plansys2_pddl_parser/Or.h | 90 +- .../include/plansys2_pddl_parser/ParamCond.h | 41 +- .../plansys2_pddl_parser/Stringreader.h | 364 ++--- .../include/plansys2_pddl_parser/Task.h | 49 +- .../plansys2_pddl_parser/TemporalAction.h | 100 +- .../plansys2_pddl_parser/TokenStruct.h | 103 +- .../include/plansys2_pddl_parser/Type.h | 280 ++-- .../include/plansys2_pddl_parser/TypeGround.h | 40 +- .../include/plansys2_pddl_parser/Utils.h | 57 +- .../include/plansys2_pddl_parser/When.h | 92 +- plansys2_pddl_parser/package.xml | 6 +- plansys2_pddl_parser/src/parser.cpp | 42 +- .../src/plansys2_pddl_parser/Action.cpp | 239 +-- .../src/plansys2_pddl_parser/And.cpp | 68 +- .../src/plansys2_pddl_parser/Derived.cpp | 78 +- .../src/plansys2_pddl_parser/Exists.cpp | 85 +- .../src/plansys2_pddl_parser/Expression.cpp | 287 ++-- .../src/plansys2_pddl_parser/Forall.cpp | 87 +- .../src/plansys2_pddl_parser/Function.cpp | 48 +- .../plansys2_pddl_parser/FunctionModifier.cpp | 135 +- .../src/plansys2_pddl_parser/Ground.cpp | 159 +- .../src/plansys2_pddl_parser/GroundFunc.cpp | 104 +- .../src/plansys2_pddl_parser/Lifted.cpp | 45 +- .../src/plansys2_pddl_parser/Not.cpp | 65 +- .../src/plansys2_pddl_parser/Oneof.cpp | 52 +- .../src/plansys2_pddl_parser/Or.cpp | 91 +- .../src/plansys2_pddl_parser/ParamCond.cpp | 31 +- .../plansys2_pddl_parser/TemporalAction.cpp | 354 ++--- .../src/plansys2_pddl_parser/TypeGround.cpp | 92 +- .../src/plansys2_pddl_parser/Utils.cpp | 287 ++-- .../src/plansys2_pddl_parser/When.cpp | 92 +- plansys2_pddl_parser/test/pddl/dom1.pddl | 2 +- plansys2_pddl_parser/test/pddl/prob1.pddl | 2 +- .../test/pddl_parser_test.cpp | 30 +- plansys2_planner/CMakeLists.txt | 1 + .../plansys2_planner/PlannerClient.hpp | 8 +- .../plansys2_planner/PlannerInterface.hpp | 1 - .../include/plansys2_planner/PlannerNode.hpp | 38 +- plansys2_planner/launch/planner_launch.py | 21 +- plansys2_planner/package.xml | 4 +- .../src/plansys2_planner/PlannerClient.cpp | 26 +- .../src/plansys2_planner/PlannerNode.cpp | 63 +- plansys2_planner/test/CMakeLists.txt | 2 +- .../test/pddl/domain_simple_constants.pddl | 6 +- .../test/pddl/problem_simple_1.pddl | 2 +- .../test/pddl/problem_simple_2.pddl | 2 +- .../test/pddl/problem_simple_3.pddl | 4 +- plansys2_planner/test/unit/CMakeLists.txt | 2 +- plansys2_planner/test/unit/planner_test.cpp | 50 +- plansys2_popf_plan_solver/CMakeLists.txt | 3 +- .../popf_plan_solver.hpp | 7 +- plansys2_popf_plan_solver/package.xml | 6 +- .../popf_plan_solver.cpp | 49 +- plansys2_popf_plan_solver/test/CMakeLists.txt | 2 +- .../test/pddl/problem_simple_1.pddl | 2 +- .../test/pddl/problem_simple_2.pddl | 2 +- .../test/pddl/problem_simple_3.pddl | 4 +- .../test/unit/CMakeLists.txt | 2 +- .../test/unit/popf_test.cpp | 64 +- plansys2_problem_expert/CMakeLists.txt | 1 + .../plansys2_problem_expert/ProblemExpert.hpp | 15 +- .../ProblemExpertClient.hpp | 62 +- .../ProblemExpertInterface.hpp | 5 +- .../ProblemExpertNode.hpp | 68 +- .../include/plansys2_problem_expert/Utils.hpp | 79 +- .../launch/problem_expert_launch.py | 32 +- plansys2_problem_expert/package.xml | 8 +- .../plansys2_problem_expert/ProblemExpert.cpp | 257 ++-- .../ProblemExpertClient.cpp | 489 +++---- .../ProblemExpertNode.cpp | 264 ++-- .../src/plansys2_problem_expert/Utils.cpp | 486 +++---- plansys2_problem_expert/test/CMakeLists.txt | 2 +- .../test/pddl/domain_charging.pddl | 6 +- .../test/pddl/domain_simple_constants.pddl | 6 +- .../test/pddl/problem_charging.pddl | 6 +- .../test/pddl/problem_empty_domain.pddl | 2 +- .../test/pddl/problem_simple_1.pddl | 2 +- .../test/pddl/problem_unexpected_syntax.pddl | 2 +- .../test/unit/problem_expert_node_test.cpp | 229 +-- .../test/unit/problem_expert_test.cpp | 209 +-- .../test/unit/utils_test.cpp | 314 ++-- plansys2_terminal/CHANGELOG.rst | 2 +- plansys2_terminal/CMakeLists.txt | 1 + plansys2_terminal/README.md | 3 +- .../include/plansys2_terminal/Terminal.hpp | 36 +- plansys2_terminal/package.xml | 6 +- .../src/plansys2_terminal/Terminal.cpp | 346 ++--- plansys2_terminal/src/terminal_node.cpp | 4 +- .../test/pddl/problem_charging.pddl | 6 +- .../test/pddl/problem_empty_domain.pddl | 2 +- plansys2_terminal/test/terminal_test.cpp | 88 +- plansys2_tests/CMakeLists.txt | 1 + plansys2_tests/README.md | 1 - .../plansys2_tests/execution_logger.hpp | 10 +- .../plansys2_tests/test_action_node.hpp | 10 +- plansys2_tests/package.xml | 6 +- .../src/plansys2_tests/execution_logger.cpp | 56 +- .../src/plansys2_tests/test_action_node.cpp | 14 +- plansys2_tests/test_1/README.md | 1 - plansys2_tests/test_1/test_1.cpp | 36 +- plansys2_tests/test_2/README.md | 3 +- plansys2_tests/test_2/pddl/problem_2.pddl | 4 +- plansys2_tests/test_2/test_2.cpp | 36 +- plansys2_tests/test_3/README.md | 1 - plansys2_tests/test_3/test_3.cpp | 174 +-- plansys2_tools/CMakeLists.txt | 1 + .../include/plansys2_logger/LoggerNode.hpp | 6 +- .../rqt_plansys2_knowledge/KnowledgeTree.hpp | 3 +- .../rqt_plansys2_knowledge/RQTKnowledge.hpp | 16 +- .../PerformersTree.hpp | 3 +- .../rqt_plansys2_performers/RQTPerformers.hpp | 16 +- .../include/rqt_plansys2_plan/PlanTree.hpp | 3 +- .../include/rqt_plansys2_plan/RQTPlan.hpp | 21 +- plansys2_tools/package.xml | 2 +- plansys2_tools/plugin_knowledge.xml | 2 +- plansys2_tools/plugin_performers.xml | 2 +- plansys2_tools/plugin_plan.xml | 2 +- plansys2_tools/scripts/rqt_plansys2_knowledge | 17 +- .../scripts/rqt_plansys2_performers | 17 +- plansys2_tools/scripts/rqt_plansys2_plan | 17 +- plansys2_tools/setup.py | 19 +- plansys2_tools/src/logger.cpp | 4 +- .../src/plansys2_logger/LoggerNode.cpp | 271 ++-- .../rqt_plansys2_knowledge/KnowledgeTree.cpp | 14 +- .../rqt_plansys2_knowledge/RQTKnowledge.cpp | 49 +- .../PerformersTree.cpp | 14 +- .../rqt_plansys2_performers/RQTPerformers.cpp | 59 +- .../src/rqt_plansys2_plan/PlanTree.cpp | 16 +- .../src/rqt_plansys2_plan/RQTPlan.cpp | 87 +- 276 files changed, 7856 insertions(+), 9067 deletions(-) create mode 100644 .clang-format create mode 100644 .github/workflows/format.yaml create mode 100644 .pre-commit-config.yaml diff --git a/.clang-format b/.clang-format new file mode 100644 index 00000000..cba7dfb7 --- /dev/null +++ b/.clang-format @@ -0,0 +1,20 @@ +--- +Language: Cpp +BasedOnStyle: Google + +SortIncludes: false +AccessModifierOffset: -2 +AlignAfterOpenBracket: AlwaysBreak +BraceWrapping: + AfterClass: true + AfterFunction: true + AfterNamespace: true + AfterStruct: true +BreakBeforeBraces: Custom +ColumnLimit: 100 +ConstructorInitializerIndentWidth: 0 +ContinuationIndentWidth: 2 +DerivePointerAlignment: false +PointerAlignment: Middle +ReflowComments: false +... diff --git a/.github/workflows/format.yaml b/.github/workflows/format.yaml new file mode 100644 index 00000000..2fc1be97 --- /dev/null +++ b/.github/workflows/format.yaml @@ -0,0 +1,28 @@ +# This is a format job. Pre-commit has a first-party GitHub action, so we use +# that: https://github.com/pre-commit/action + +name: Format + +on: + workflow_dispatch: + pull_request: + push: + branches: + - main + +jobs: + pre-commit: + name: pre-commit + runs-on: ubuntu-22.04 + steps: + - uses: actions/checkout@v2 + - uses: actions/setup-python@v2 + - name: Install clang-format + run: sudo apt-get install clang-format + - uses: pre-commit/action@v2.0.3 + id: precommit + - name: Upload pre-commit changes + if: failure() && steps.precommit.outcome == 'failure' + uses: rhaschke/upload-git-patch-action@main + with: + name: pre-commit diff --git a/.github/workflows/foxy-devel.yaml b/.github/workflows/foxy-devel.yaml index d9102415..0cb6947e 100644 --- a/.github/workflows/foxy-devel.yaml +++ b/.github/workflows/foxy-devel.yaml @@ -7,7 +7,7 @@ on: push: branches: - foxy-devel - + jobs: build-and-test: runs-on: ${{ matrix.os }} diff --git a/.github/workflows/galactic-devel.yaml b/.github/workflows/galactic-devel.yaml index 5bad91b9..1d971848 100644 --- a/.github/workflows/galactic-devel.yaml +++ b/.github/workflows/galactic-devel.yaml @@ -7,7 +7,7 @@ on: push: branches: - galactic-devel - + jobs: build-and-test: runs-on: ${{ matrix.os }} diff --git a/.github/workflows/humble-devel.yaml b/.github/workflows/humble-devel.yaml index 88b31af7..e90eb186 100644 --- a/.github/workflows/humble-devel.yaml +++ b/.github/workflows/humble-devel.yaml @@ -7,7 +7,7 @@ on: push: branches: - humble-devel - + jobs: build-and-test: runs-on: ${{ matrix.os }} diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 00000000..950795ee --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,54 @@ +# To use: +# +# pre-commit run -a +# +# Or: +# +# pre-commit install # (runs every time you commit in git) +# +# To update this file: +# +# pre-commit autoupdate +# +# See https://github.com/pre-commit/pre-commit + +repos: + # Standard hooks + - repo: https://github.com/pre-commit/pre-commit-hooks + rev: v3.4.0 + hooks: + - id: check-added-large-files + - id: check-case-conflict + - id: check-json + - id: check-merge-conflict + - id: check-symlinks + - id: check-toml + - id: check-yaml + - id: debug-statements + - id: destroyed-symlinks + - id: detect-private-key + - id: end-of-file-fixer + - id: mixed-line-ending + - id: pretty-format-json + - id: trailing-whitespace + + - repo: https://github.com/psf/black + rev: 22.3.0 + hooks: + - id: black + + - repo: local + hooks: + - id: clang-format + name: clang-format + description: Format files with ClangFormat. + entry: clang-format-12 + language: system + files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$ + args: ['-fallback-style=none', '-i'] + + - repo: https://github.com/codespell-project/codespell + rev: v2.0.0 + hooks: + - id: codespell + exclude: CHANGELOG.rst diff --git a/README.md b/README.md index 842532a6..80b6332c 100644 --- a/README.md +++ b/README.md @@ -25,9 +25,9 @@ This project is the result of several years of experience in the development of We hope that this software helps to include planning in more Robotics projects, offering simple and powerful software to generate intelligent behaviors for robots. -Please, cite us if you use PlanSys2 in your reseach: +Please, cite us if you use PlanSys2 in your research: -``` +``` @INPROCEEDINGS {PlanSys2, author = "Francisco Mart{\'{\i}}n and Jonatan Gin{\'{e}}s and Francisco J. Rodr{\'{i}}guez and Vicente Matell{\'{a}}n", diff --git a/plansys2_bringup/CMakeLists.txt b/plansys2_bringup/CMakeLists.txt index 38c81db7..ebca2ae0 100644 --- a/plansys2_bringup/CMakeLists.txt +++ b/plansys2_bringup/CMakeLists.txt @@ -29,6 +29,7 @@ target_compile_definitions(plansys2_node PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTI if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) + set(ament_cmake_uncrustify_FOUND TRUE) ament_lint_auto_find_test_dependencies() endif() diff --git a/plansys2_bringup/launch/plansys2_bringup_launch_distributed.py b/plansys2_bringup/launch/plansys2_bringup_launch_distributed.py index 0d5e1beb..9491ac3a 100644 --- a/plansys2_bringup/launch/plansys2_bringup_launch_distributed.py +++ b/plansys2_bringup/launch/plansys2_bringup_launch_distributed.py @@ -1,3 +1,4 @@ +# -*- coding: utf-8 -*- # Copyright 2019 Intelligent Robotics Lab # # Licensed under the Apache License, Version 2.0 (the "License"); @@ -24,51 +25,60 @@ def generate_launch_description(): - bringup_dir = get_package_share_directory('plansys2_bringup') + bringup_dir = get_package_share_directory("plansys2_bringup") # Create the launch configuration variables - model_file = LaunchConfiguration('model_file') - namespace = LaunchConfiguration('namespace') - params_file = LaunchConfiguration('params_file') - action_bt_file = LaunchConfiguration('action_bt_file') - start_action_bt_file = LaunchConfiguration('start_action_bt_file') - end_action_bt_file = LaunchConfiguration('end_action_bt_file') + model_file = LaunchConfiguration("model_file") + namespace = LaunchConfiguration("namespace") + params_file = LaunchConfiguration("params_file") + action_bt_file = LaunchConfiguration("action_bt_file") + start_action_bt_file = LaunchConfiguration("start_action_bt_file") + end_action_bt_file = LaunchConfiguration("end_action_bt_file") bt_builder_plugin = LaunchConfiguration("bt_builder_plugin") declare_model_file_cmd = DeclareLaunchArgument( - 'model_file', - description='PDDL Model file') + "model_file", description="PDDL Model file" + ) declare_namespace_cmd = DeclareLaunchArgument( - 'namespace', - default_value='', - description='Namespace') + "namespace", default_value="", description="Namespace" + ) declare_params_file_cmd = DeclareLaunchArgument( - 'params_file', - default_value=os.path.join(bringup_dir, 'params', 'plansys2_params.yaml'), - description='Full path to the ROS2 parameters file to use for all launched nodes') + "params_file", + default_value=os.path.join(bringup_dir, "params", "plansys2_params.yaml"), + description="Full path to the ROS2 parameters file to use for all launched nodes", + ) declare_action_bt_file_cmd = DeclareLaunchArgument( - 'action_bt_file', + "action_bt_file", default_value=os.path.join( - get_package_share_directory('plansys2_executor'), - 'behavior_trees', 'plansys2_action_bt.xml'), - description='BT representing a PDDL action') + get_package_share_directory("plansys2_executor"), + "behavior_trees", + "plansys2_action_bt.xml", + ), + description="BT representing a PDDL action", + ) declare_start_action_bt_file_cmd = DeclareLaunchArgument( - 'start_action_bt_file', + "start_action_bt_file", default_value=os.path.join( - get_package_share_directory('plansys2_executor'), - 'behavior_trees', 'plansys2_start_action_bt.xml'), - description='BT representing a PDDL start action') + get_package_share_directory("plansys2_executor"), + "behavior_trees", + "plansys2_start_action_bt.xml", + ), + description="BT representing a PDDL start action", + ) declare_end_action_bt_file_cmd = DeclareLaunchArgument( - 'end_action_bt_file', + "end_action_bt_file", default_value=os.path.join( - get_package_share_directory('plansys2_executor'), - 'behavior_trees', 'plansys2_end_action_bt.xml'), - description='BT representing a PDDL end action') + get_package_share_directory("plansys2_executor"), + "behavior_trees", + "plansys2_end_action_bt.xml", + ), + description="BT representing a PDDL end action", + ) declare_bt_builder_plugin_cmd = DeclareLaunchArgument( "bt_builder_plugin", @@ -77,58 +87,72 @@ def generate_launch_description(): ) domain_expert_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join( - get_package_share_directory('plansys2_domain_expert'), - 'launch', - 'domain_expert_launch.py')), + PythonLaunchDescriptionSource( + os.path.join( + get_package_share_directory("plansys2_domain_expert"), + "launch", + "domain_expert_launch.py", + ) + ), launch_arguments={ - 'model_file': model_file, - 'namespace': namespace, - 'params_file': params_file - }.items()) + "model_file": model_file, + "namespace": namespace, + "params_file": params_file, + }.items(), + ) problem_expert_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join( - get_package_share_directory('plansys2_problem_expert'), - 'launch', - 'problem_expert_launch.py')), + PythonLaunchDescriptionSource( + os.path.join( + get_package_share_directory("plansys2_problem_expert"), + "launch", + "problem_expert_launch.py", + ) + ), launch_arguments={ - 'model_file': model_file, - 'namespace': namespace, - 'params_file': params_file - }.items()) + "model_file": model_file, + "namespace": namespace, + "params_file": params_file, + }.items(), + ) planner_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join( - get_package_share_directory('plansys2_planner'), - 'launch', - 'planner_launch.py')), - launch_arguments={ - 'namespace': namespace, - 'params_file': params_file - }.items()) + PythonLaunchDescriptionSource( + os.path.join( + get_package_share_directory("plansys2_planner"), + "launch", + "planner_launch.py", + ) + ), + launch_arguments={"namespace": namespace, "params_file": params_file}.items(), + ) executor_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join( - get_package_share_directory('plansys2_executor'), - 'launch', - 'executor_launch.py')), + PythonLaunchDescriptionSource( + os.path.join( + get_package_share_directory("plansys2_executor"), + "launch", + "executor_launch.py", + ) + ), launch_arguments={ - 'namespace': namespace, - 'params_file': params_file, - 'default_action_bt_xml_filename': action_bt_file, - 'default_start_action_bt_xml_filename': start_action_bt_file, - 'default_end_action_bt_xml_filename': end_action_bt_file, - 'bt_builder_plugin': bt_builder_plugin, - }.items()) + "namespace": namespace, + "params_file": params_file, + "default_action_bt_xml_filename": action_bt_file, + "default_start_action_bt_xml_filename": start_action_bt_file, + "default_end_action_bt_xml_filename": end_action_bt_file, + "bt_builder_plugin": bt_builder_plugin, + }.items(), + ) lifecycle_manager_cmd = Node( - package='plansys2_lifecycle_manager', - executable='lifecycle_manager_node', - name='lifecycle_manager_node', + package="plansys2_lifecycle_manager", + executable="lifecycle_manager_node", + name="lifecycle_manager_node", namespace=namespace, - output='screen', - parameters=[]) + output="screen", + parameters=[], + ) # Create the launch description and populate ld = LaunchDescription() diff --git a/plansys2_bringup/launch/plansys2_bringup_launch_monolithic.py b/plansys2_bringup/launch/plansys2_bringup_launch_monolithic.py index f70da401..f6bb818c 100644 --- a/plansys2_bringup/launch/plansys2_bringup_launch_monolithic.py +++ b/plansys2_bringup/launch/plansys2_bringup_launch_monolithic.py @@ -1,3 +1,4 @@ +# -*- coding: utf-8 -*- # Copyright 2019 Intelligent Robotics Lab # # Licensed under the Apache License, Version 2.0 (the "License"); @@ -24,51 +25,60 @@ def generate_launch_description(): # Get the launch directory - bringup_dir = get_package_share_directory('plansys2_bringup') + bringup_dir = get_package_share_directory("plansys2_bringup") # Create the launch configuration variables - model_file = LaunchConfiguration('model_file') - namespace = LaunchConfiguration('namespace') - params_file = LaunchConfiguration('params_file') - action_bt_file = LaunchConfiguration('action_bt_file') + model_file = LaunchConfiguration("model_file") + namespace = LaunchConfiguration("namespace") + params_file = LaunchConfiguration("params_file") + action_bt_file = LaunchConfiguration("action_bt_file") start_action_bt_file = LaunchConfiguration("start_action_bt_file") end_action_bt_file = LaunchConfiguration("end_action_bt_file") bt_builder_plugin = LaunchConfiguration("bt_builder_plugin") declare_model_file_cmd = DeclareLaunchArgument( - 'model_file', - description='PDDL Model file') + "model_file", description="PDDL Model file" + ) declare_namespace_cmd = DeclareLaunchArgument( - 'namespace', - default_value='', - description='Namespace') + "namespace", default_value="", description="Namespace" + ) declare_params_file_cmd = DeclareLaunchArgument( - 'params_file', - default_value=os.path.join(bringup_dir, 'params', 'plansys2_params.yaml'), - description='Full path to the ROS2 parameters file to use for all launched nodes') + "params_file", + default_value=os.path.join(bringup_dir, "params", "plansys2_params.yaml"), + description="Full path to the ROS2 parameters file to use for all launched nodes", + ) declare_action_bt_file_cmd = DeclareLaunchArgument( - 'action_bt_file', + "action_bt_file", default_value=os.path.join( - get_package_share_directory('plansys2_executor'), - 'behavior_trees', 'plansys2_action_bt.xml'), - description='BT representing a PDDL action') + get_package_share_directory("plansys2_executor"), + "behavior_trees", + "plansys2_action_bt.xml", + ), + description="BT representing a PDDL action", + ) declare_start_action_bt_file_cmd = DeclareLaunchArgument( - 'start_action_bt_file', + "start_action_bt_file", default_value=os.path.join( - get_package_share_directory('plansys2_executor'), - 'behavior_trees', 'plansys2_start_action_bt.xml'), - description='BT representing a PDDL start action') + get_package_share_directory("plansys2_executor"), + "behavior_trees", + "plansys2_start_action_bt.xml", + ), + description="BT representing a PDDL start action", + ) declare_end_action_bt_file_cmd = DeclareLaunchArgument( - 'end_action_bt_file', + "end_action_bt_file", default_value=os.path.join( - get_package_share_directory('plansys2_executor'), - 'behavior_trees', 'plansys2_end_action_bt.xml'), - description='BT representing a PDDL end action') + get_package_share_directory("plansys2_executor"), + "behavior_trees", + "plansys2_end_action_bt.xml", + ), + description="BT representing a PDDL end action", + ) declare_bt_builder_plugin_cmd = DeclareLaunchArgument( "bt_builder_plugin", @@ -77,20 +87,21 @@ def generate_launch_description(): ) plansys2_node_cmd = Node( - package='plansys2_bringup', - executable='plansys2_node', - output='screen', + package="plansys2_bringup", + executable="plansys2_node", + output="screen", namespace=namespace, parameters=[ - { - 'model_file': model_file, - 'default_action_bt_xml_filename': action_bt_file, - 'default_start_action_bt_xml_filename': start_action_bt_file, - 'default_end_action_bt_xml_filename': end_action_bt_file, - 'bt_builder_plugin': bt_builder_plugin, - }, - params_file - ]) + { + "model_file": model_file, + "default_action_bt_xml_filename": action_bt_file, + "default_start_action_bt_xml_filename": start_action_bt_file, + "default_end_action_bt_xml_filename": end_action_bt_file, + "bt_builder_plugin": bt_builder_plugin, + }, + params_file, + ], + ) # Create the launch description and populate ld = LaunchDescription() diff --git a/plansys2_bringup/package.xml b/plansys2_bringup/package.xml index 0ca90db5..70fa66c2 100644 --- a/plansys2_bringup/package.xml +++ b/plansys2_bringup/package.xml @@ -3,11 +3,11 @@ plansys2_bringup 2.0.8 - + Bringup scripts and configurations for the ROS2 Planning System Francisco Martin Rico - + Apache License, Version 2.0 ament_cmake diff --git a/plansys2_bringup/src/plansys2_node.cpp b/plansys2_bringup/src/plansys2_node.cpp index b0c0bce2..06169dcb 100644 --- a/plansys2_bringup/src/plansys2_node.cpp +++ b/plansys2_bringup/src/plansys2_node.cpp @@ -13,18 +13,16 @@ // limitations under the License. #include +#include #include #include -#include - -#include "rclcpp/rclcpp.hpp" #include "plansys2_domain_expert/DomainExpertNode.hpp" -#include "plansys2_problem_expert/ProblemExpertNode.hpp" -#include "plansys2_planner/PlannerNode.hpp" #include "plansys2_executor/ExecutorNode.hpp" - #include "plansys2_lifecycle_manager/lifecycle_manager.hpp" +#include "plansys2_planner/PlannerNode.hpp" +#include "plansys2_problem_expert/ProblemExpertNode.hpp" +#include "rclcpp/rclcpp.hpp" int main(int argc, char ** argv) { @@ -45,14 +43,14 @@ int main(int argc, char ** argv) exe.add_node(executor_node->get_node_base_interface()); std::map> manager_nodes; - manager_nodes["domain_expert"] = std::make_shared( - "domain_expert_lc_mngr", "domain_expert"); - manager_nodes["problem_expert"] = std::make_shared( - "problem_expert_lc_mngr", "problem_expert"); - manager_nodes["planner"] = std::make_shared( - "planner_lc_mngr", "planner"); - manager_nodes["executor"] = std::make_shared( - "executor_lc_mngr", "executor"); + manager_nodes["domain_expert"] = + std::make_shared("domain_expert_lc_mngr", "domain_expert"); + manager_nodes["problem_expert"] = + std::make_shared("problem_expert_lc_mngr", "problem_expert"); + manager_nodes["planner"] = + std::make_shared("planner_lc_mngr", "planner"); + manager_nodes["executor"] = + std::make_shared("executor_lc_mngr", "executor"); for (auto & manager_node : manager_nodes) { manager_node.second->init(); @@ -65,9 +63,7 @@ int main(int argc, char ** argv) exe.spin_until_future_complete(startup_future); if (!startup_future.get()) { - RCLCPP_ERROR( - rclcpp::get_logger("plansys2_bringup"), - "Failed to start plansys2!"); + RCLCPP_ERROR(rclcpp::get_logger("plansys2_bringup"), "Failed to start plansys2!"); rclcpp::shutdown(); return -1; } diff --git a/plansys2_bt_actions/CMakeLists.txt b/plansys2_bt_actions/CMakeLists.txt index 8879b22e..98b8cb0f 100644 --- a/plansys2_bt_actions/CMakeLists.txt +++ b/plansys2_bt_actions/CMakeLists.txt @@ -63,6 +63,7 @@ install(TARGETS if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) + set(ament_cmake_uncrustify_FOUND TRUE) ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest REQUIRED) diff --git a/plansys2_bt_actions/README.md b/plansys2_bt_actions/README.md index f7f0595f..b6c0acdf 100644 --- a/plansys2_bt_actions/README.md +++ b/plansys2_bt_actions/README.md @@ -20,7 +20,7 @@ Files created by the `.fbl` and minitrace loggers are stored in `/tmp/ @@ -34,29 +34,26 @@ namespace plansys2 { - class BTAction : public plansys2::ActionExecutorClient { public: - explicit BTAction( - const std::string & action, - const std::chrono::nanoseconds & rate); + explicit BTAction(const std::string & action, const std::chrono::nanoseconds & rate); - const std::string & getActionName() const {return action_;} - const std::string & getBTFile() const {return bt_xml_file_;} + const std::string & getActionName() const { return action_; } + const std::string & getBTFile() const { return bt_xml_file_; } protected: - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_configure(const rclcpp_lifecycle::State & previous_state); + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state); - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_cleanup(const rclcpp_lifecycle::State & previous_state); + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup( + const rclcpp_lifecycle::State & previous_state); - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_activate(const rclcpp_lifecycle::State & previous_state); + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state); - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - on_deactivate(const rclcpp_lifecycle::State & previous_state); + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state); void do_work(); diff --git a/plansys2_bt_actions/include/plansys2_bt_actions/BTActionNode.hpp b/plansys2_bt_actions/include/plansys2_bt_actions/BTActionNode.hpp index 26ef7c56..e48f2eb9 100644 --- a/plansys2_bt_actions/include/plansys2_bt_actions/BTActionNode.hpp +++ b/plansys2_bt_actions/include/plansys2_bt_actions/BTActionNode.hpp @@ -20,21 +20,19 @@ #include "behaviortree_cpp_v3/action_node.h" #include "rclcpp/rclcpp.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" #include "rclcpp_action/rclcpp_action.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" namespace plansys2 { - using namespace std::chrono_literals; // NOLINT -template +template class BtActionNode : public BT::ActionNodeBase { public: BtActionNode( - const std::string & xml_tag_name, - const std::string & action_name, + const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf) : BT::ActionNodeBase(xml_tag_name, conf), action_name_(action_name) { @@ -58,14 +56,13 @@ class BtActionNode : public BT::ActionNodeBase BtActionNode() = delete; - virtual ~BtActionNode() - { - } + virtual ~BtActionNode() {} // Create instance of an action server bool createActionClient(const std::string & action_name) { - // Now that we have the ROS node to use, create the action client for this BT action + // Now that we have the ROS node to use, create the action client for this + // BT action action_client_ = rclcpp_action::create_client(node_, action_name); // Make sure the server is actually there before continuing @@ -75,74 +72,55 @@ class BtActionNode : public BT::ActionNodeBase if (!success_waiting) { RCLCPP_ERROR( - node_->get_logger(), - "Timeout (%ld secs) waiting for \"%s\" action server", - server_timeout_.count() * 1000, - action_name.c_str()); + node_->get_logger(), "Timeout (%ld secs) waiting for \"%s\" action server", + server_timeout_.count() * 1000, action_name.c_str()); } return success_waiting; } - // Any subclass of BtActionNode that accepts parameters must provide a providedPorts method - // and call providedBasicPorts in it. + // Any subclass of BtActionNode that accepts parameters must provide a + // providedPorts method and call providedBasicPorts in it. static BT::PortsList providedBasicPorts(BT::PortsList addition) { BT::PortsList basic = { BT::InputPort("server_name", "Action server name"), BT::InputPort( - "server_timeout", - 5.0, - "The amount of time to wait for a response from the action server, in seconds") - }; + "server_timeout", 5.0, + "The amount of time to wait for a response from " + "the action server, in seconds")}; basic.insert(addition.begin(), addition.end()); return basic; } - static BT::PortsList providedPorts() - { - return providedBasicPorts({}); - } + static BT::PortsList providedPorts() { return providedBasicPorts({}); } // Derived classes can override any of the following methods to hook into the // processing for the action: on_tick, and on_success - // Could do dynamic checks, such as getting updates to values on the blackboard - // Can also update variable goal_updated_ to request a new goal - virtual BT::NodeStatus on_tick() - { - return BT::NodeStatus::RUNNING; - } + // Could do dynamic checks, such as getting updates to values on the + // blackboard Can also update variable goal_updated_ to request a new goal + virtual BT::NodeStatus on_tick() { return BT::NodeStatus::RUNNING; } // Provides the opportunity for derived classes to log feedback, update the // goal, or cancel the goal - virtual void on_feedback( - const std::shared_ptr feedback) + virtual void on_feedback(const std::shared_ptr feedback) { (void)feedback; } - // Called upon successful completion of the action. A derived class can override this - // method to put a value on the blackboard, for example. - virtual BT::NodeStatus on_success() - { - return BT::NodeStatus::SUCCESS; - } + // Called upon successful completion of the action. A derived class can + // override this method to put a value on the blackboard, for example. + virtual BT::NodeStatus on_success() { return BT::NodeStatus::SUCCESS; } - // Called when a the action is aborted. By default, the node will return FAILURE. - // The user may override it to return another value, instead. - virtual BT::NodeStatus on_aborted() - { - return BT::NodeStatus::FAILURE; - } + // Called when a the action is aborted. By default, the node will return + // FAILURE. The user may override it to return another value, instead. + virtual BT::NodeStatus on_aborted() { return BT::NodeStatus::FAILURE; } - // Called when a the action is cancelled. By default, the node will return SUCCESS. - // The user may override it to return another value, instead. - virtual BT::NodeStatus on_cancelled() - { - return BT::NodeStatus::SUCCESS; - } + // Called when a the action is cancelled. By default, the node will return + // SUCCESS. The user may override it to return another value, instead. + virtual BT::NodeStatus on_cancelled() { return BT::NodeStatus::SUCCESS; } // The main override required by a BT action BT::NodeStatus tick() override @@ -179,9 +157,9 @@ class BtActionNode : public BT::ActionNodeBase // The following code corresponds to the "RUNNING" loop if (rclcpp::ok() && !goal_result_available_) { auto goal_status = goal_handle_->get_status(); - if (goal_updated_ && (goal_status == action_msgs::msg::GoalStatus::STATUS_EXECUTING || - goal_status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED)) - { + if ( + goal_updated_ && (goal_status == action_msgs::msg::GoalStatus::STATUS_EXECUTING || + goal_status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED)) { goal_updated_ = false; if (!on_new_goal_received()) { cancel_goal(); @@ -235,13 +213,12 @@ class BtActionNode : public BT::ActionNodeBase void cancel_goal() { auto future_cancel = action_client_->async_cancel_goal(goal_handle_); - if (rclcpp::spin_until_future_complete( + if ( + rclcpp::spin_until_future_complete( node_->get_node_base_interface(), future_cancel, server_timeout_) != - rclcpp::FutureReturnCode::SUCCESS) - { + rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR( - node_->get_logger(), - "Failed to cancel action server for %s", action_name_.c_str()); + node_->get_logger(), "Failed to cancel action server for %s", action_name_.c_str()); } } @@ -260,7 +237,6 @@ class BtActionNode : public BT::ActionNodeBase status == action_msgs::msg::GoalStatus::STATUS_EXECUTING; } - bool on_new_goal_received() { goal_result_available_ = false; @@ -268,42 +244,37 @@ class BtActionNode : public BT::ActionNodeBase send_goal_options.result_callback = [this](const typename rclcpp_action::ClientGoalHandle::WrappedResult & result) { // TODO(#1652): a work around until rcl_action interface is updated - // if goal ids are not matched, the older goal call this callback so ignore the result - // if matched, it must be processed (including aborted) + // if goal ids are not matched, the older goal call this callback so + // ignore the result if matched, it must be processed (including + // aborted) if (this->goal_handle_->get_goal_id() == result.goal_id) { goal_result_available_ = true; result_ = result; } }; send_goal_options.feedback_callback = - [this](typename rclcpp_action::ClientGoalHandle::SharedPtr, + [this]( + typename rclcpp_action::ClientGoalHandle::SharedPtr, const std::shared_ptr feedback) { on_feedback(feedback); }; - RCLCPP_INFO( - node_->get_logger(), - "Sending goal to action server %s", - action_name_.c_str()); + RCLCPP_INFO(node_->get_logger(), "Sending goal to action server %s", action_name_.c_str()); auto future_goal_handle = action_client_->async_send_goal(goal_, send_goal_options); - if (rclcpp::spin_until_future_complete( + if ( + rclcpp::spin_until_future_complete( node_->get_node_base_interface(), future_goal_handle, server_timeout_) != - rclcpp::FutureReturnCode::SUCCESS) - { + rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR( - node_->get_logger(), - "Failed to send goal to action server %s", - action_name_.c_str()); + node_->get_logger(), "Failed to send goal to action server %s", action_name_.c_str()); return false; } goal_handle_ = future_goal_handle.get(); if (!goal_handle_) { RCLCPP_ERROR( - node_->get_logger(), - "Goal was rejected by action server %s", - action_name_.c_str()); + node_->get_logger(), "Goal was rejected by action server %s", action_name_.c_str()); return false; } @@ -313,9 +284,11 @@ class BtActionNode : public BT::ActionNodeBase void increment_recovery_count() { int recovery_count = 0; - config().blackboard->get("number_recoveries", recovery_count); // NOLINT + config().blackboard->get("number_recoveries", + recovery_count); // NOLINT recovery_count += 1; - config().blackboard->set("number_recoveries", recovery_count); // NOLINT + config().blackboard->set("number_recoveries", + recovery_count); // NOLINT } std::string action_name_; @@ -336,7 +309,6 @@ class BtActionNode : public BT::ActionNodeBase std::chrono::milliseconds server_timeout_; }; - } // namespace plansys2 #endif // PLANSYS2_BT_ACTIONS__BTACTIONNODE_HPP_ diff --git a/plansys2_bt_actions/package.xml b/plansys2_bt_actions/package.xml index cd4e2bca..19abe999 100644 --- a/plansys2_bt_actions/package.xml +++ b/plansys2_bt_actions/package.xml @@ -3,11 +3,11 @@ plansys2_bt_actions 2.0.8 - + This package contains the Problem Expert module for the ROS2 Planning System Francisco Martin Rico - + Apache License, Version 2.0 ament_cmake diff --git a/plansys2_bt_actions/src/bt_action_node.cpp b/plansys2_bt_actions/src/bt_action_node.cpp index fc544627..5b353c80 100644 --- a/plansys2_bt_actions/src/bt_action_node.cpp +++ b/plansys2_bt_actions/src/bt_action_node.cpp @@ -18,7 +18,6 @@ #include "plansys2_bt_actions/BTAction.hpp" #include "rclcpp/rclcpp.hpp" - using namespace std::chrono_literals; int main(int argc, char ** argv) diff --git a/plansys2_bt_actions/src/plansys2_bt_actions/BTAction.cpp b/plansys2_bt_actions/src/plansys2_bt_actions/BTAction.cpp index 1464c03f..22c15b58 100644 --- a/plansys2_bt_actions/src/plansys2_bt_actions/BTAction.cpp +++ b/plansys2_bt_actions/src/plansys2_bt_actions/BTAction.cpp @@ -12,30 +12,27 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "plansys2_bt_actions/BTAction.hpp" + +#include +#include #include #include -#include -#include +#include +#include #include +#include #include -#include -#include #include "behaviortree_cpp_v3/utils/shared_library.h" -#include "plansys2_bt_actions/BTAction.hpp" namespace plansys2 { - -BTAction::BTAction( - const std::string & action, - const std::chrono::nanoseconds & rate) +BTAction::BTAction(const std::string & action, const std::chrono::nanoseconds & rate) : ActionExecutorClient(action, rate) { declare_parameter("bt_xml_file", ""); - declare_parameter>( - "plugins", std::vector({})); + declare_parameter>("plugins", std::vector({})); declare_parameter("bt_file_logging", false); declare_parameter("bt_minitrace_logging", false); #ifdef ZMQ_FOUND @@ -46,8 +43,8 @@ BTAction::BTAction( #endif } -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -BTAction::on_configure(const rclcpp_lifecycle::State & previous_state) +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn BTAction::on_configure( + const rclcpp_lifecycle::State & previous_state) { get_parameter("action_name", action_); get_parameter("bt_xml_file", bt_xml_file_); @@ -75,22 +72,20 @@ BTAction::on_configure(const rclcpp_lifecycle::State & previous_state) return ActionExecutorClient::on_configure(previous_state); } -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -BTAction::on_cleanup(const rclcpp_lifecycle::State & previous_state) +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn BTAction::on_cleanup( + const rclcpp_lifecycle::State & previous_state) { publisher_zmq_.reset(); return ActionExecutorClient::on_cleanup(previous_state); } -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -BTAction::on_activate(const rclcpp_lifecycle::State & previous_state) +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn BTAction::on_activate( + const rclcpp_lifecycle::State & previous_state) { try { tree_ = factory_.createTreeFromFile(bt_xml_file_, blackboard_); } catch (const std::exception & ex) { - RCLCPP_ERROR_STREAM( - get_logger(), - "Failed to create BT with exception: " << ex.what()); + RCLCPP_ERROR_STREAM(get_logger(), "Failed to create BT with exception: " << ex.what()); RCLCPP_ERROR(get_logger(), "Transition to activate failed"); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; } @@ -100,33 +95,26 @@ BTAction::on_activate(const rclcpp_lifecycle::State & previous_state) blackboard_->set(argname, get_arguments()[i]); } - if (get_parameter("bt_file_logging").as_bool() || - get_parameter("bt_minitrace_logging").as_bool()) - { + if ( + get_parameter("bt_file_logging").as_bool() || get_parameter("bt_minitrace_logging").as_bool()) { auto temp_path = std::filesystem::temp_directory_path(); std::filesystem::path node_name_path = get_name(); std::filesystem::create_directories(temp_path / node_name_path); - auto now_time_t = - std::chrono::system_clock::to_time_t(std::chrono::system_clock::now()); + auto now_time_t = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now()); std::stringstream filename; filename << "/tmp/" << get_name() << "/bt_trace_"; filename << std::put_time(std::localtime(&now_time_t), "%Y_%m_%d__%H_%M_%S"); if (get_parameter("bt_file_logging").as_bool()) { std::string filename_extension = filename.str() + ".fbl"; - RCLCPP_INFO_STREAM( - get_logger(), - "Logging to file: " << filename_extension); - bt_file_logger_ = - std::make_unique(tree_, filename_extension.c_str()); + RCLCPP_INFO_STREAM(get_logger(), "Logging to file: " << filename_extension); + bt_file_logger_ = std::make_unique(tree_, filename_extension.c_str()); } if (get_parameter("bt_minitrace_logging").as_bool()) { std::string filename_extension = filename.str() + ".json"; - RCLCPP_INFO_STREAM( - get_logger(), - "Logging to file: " << filename_extension); + RCLCPP_INFO_STREAM(get_logger(), "Logging to file: " << filename_extension); bt_minitrace_logger_ = std::make_unique(tree_, filename_extension.c_str()); } @@ -146,13 +134,12 @@ BTAction::on_activate(const rclcpp_lifecycle::State & previous_state) } else if (get_parameter("enable_groot_monitoring").as_bool()) { RCLCPP_DEBUG( get_logger(), - "[%s] Groot monitoring: Publisher port: %d, Server port: %d, Max msgs per second: %d", + "[%s] Groot monitoring: Publisher port: %d, Server port: %d, " + "Max msgs per second: %d", get_name(), publisher_port, server_port, max_msgs_per_second); try { publisher_zmq_.reset( - new BT::PublisherZMQ( - tree_, max_msgs_per_second, publisher_port, - server_port)); + new BT::PublisherZMQ(tree_, max_msgs_per_second, publisher_port, server_port)); } catch (const BT::LogicError & exc) { RCLCPP_ERROR(get_logger(), "ZMQ error: %s", exc.what()); } @@ -163,8 +150,8 @@ BTAction::on_activate(const rclcpp_lifecycle::State & previous_state) return ActionExecutorClient::on_activate(previous_state); } -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -BTAction::on_deactivate(const rclcpp_lifecycle::State & previous_state) +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn BTAction::on_deactivate( + const rclcpp_lifecycle::State & previous_state) { publisher_zmq_.reset(); bt_minitrace_logger_.reset(); @@ -174,8 +161,7 @@ BTAction::on_deactivate(const rclcpp_lifecycle::State & previous_state) return ActionExecutorClient::on_deactivate(previous_state); } -void -BTAction::do_work() +void BTAction::do_work() { if (!finished_) { BT::NodeStatus result; diff --git a/plansys2_bt_actions/test/behavior_tree/CloseGripper.cpp b/plansys2_bt_actions/test/behavior_tree/CloseGripper.cpp index 33a7eef7..912a2f6a 100644 --- a/plansys2_bt_actions/test/behavior_tree/CloseGripper.cpp +++ b/plansys2_bt_actions/test/behavior_tree/CloseGripper.cpp @@ -12,31 +12,23 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include - #include "CloseGripper.hpp" +#include +#include + #include "behaviortree_cpp_v3/behavior_tree.h" namespace plansys2_bt_tests { - -CloseGripper::CloseGripper( - const std::string & xml_tag_name, - const BT::NodeConfiguration & conf) +CloseGripper::CloseGripper(const std::string & xml_tag_name, const BT::NodeConfiguration & conf) : BT::ActionNodeBase(xml_tag_name, conf), counter_(0) { } -void -CloseGripper::halt() -{ - std::cout << "CloseGripper halt" << std::endl; -} +void CloseGripper::halt() { std::cout << "CloseGripper halt" << std::endl; } -BT::NodeStatus -CloseGripper::tick() +BT::NodeStatus CloseGripper::tick() { std::cout << "CloseGripper tick " << counter_ << std::endl; diff --git a/plansys2_bt_actions/test/behavior_tree/CloseGripper.hpp b/plansys2_bt_actions/test/behavior_tree/CloseGripper.hpp index 60b43388..77fd62d1 100644 --- a/plansys2_bt_actions/test/behavior_tree/CloseGripper.hpp +++ b/plansys2_bt_actions/test/behavior_tree/CloseGripper.hpp @@ -15,28 +15,22 @@ #ifndef BEHAVIOR_TREE__CLOSEGRIPPER_HPP_ #define BEHAVIOR_TREE__CLOSEGRIPPER_HPP_ -#include +#include +#include -#include "behaviortree_cpp_v3/behavior_tree.h" -#include "behaviortree_cpp_v3/bt_factory.h" +#include namespace plansys2_bt_tests { - class CloseGripper : public BT::ActionNodeBase { public: - explicit CloseGripper( - const std::string & xml_tag_name, - const BT::NodeConfiguration & conf); + explicit CloseGripper(const std::string & xml_tag_name, const BT::NodeConfiguration & conf); void halt(); BT::NodeStatus tick(); - static BT::PortsList providedPorts() - { - return BT::PortsList({}); - } + static BT::PortsList providedPorts() { return BT::PortsList({}); } private: int counter_; diff --git a/plansys2_bt_actions/test/behavior_tree/FailureNodes.cpp b/plansys2_bt_actions/test/behavior_tree/FailureNodes.cpp index 9cc2fa8e..e7bb73e1 100644 --- a/plansys2_bt_actions/test/behavior_tree/FailureNodes.cpp +++ b/plansys2_bt_actions/test/behavior_tree/FailureNodes.cpp @@ -12,32 +12,24 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "FailureNodes.hpp" + #include +#include #include "behaviortree_cpp_v3/bt_factory.h" -#include "FailureNodes.hpp" - BT_REGISTER_NODES(factory) { - BT::NodeBuilder builder = - [](const std::string & name, const BT::NodeConfiguration & config) - { - return std::make_unique( - name, "move", config); - }; + BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) { + return std::make_unique(name, "move", config); + }; - factory.registerBuilder( - "OnTickFail", builder); + factory.registerBuilder("OnTickFail", builder); - builder = - [](const std::string & name, const BT::NodeConfiguration & config) - { - return std::make_unique( - name, "move", config); - }; + builder = [](const std::string & name, const BT::NodeConfiguration & config) { + return std::make_unique(name, "move", config); + }; - factory.registerBuilder( - "OnFeedbackFail", builder); + factory.registerBuilder("OnFeedbackFail", builder); } diff --git a/plansys2_bt_actions/test/behavior_tree/FailureNodes.hpp b/plansys2_bt_actions/test/behavior_tree/FailureNodes.hpp index f495a8e5..fde87b10 100644 --- a/plansys2_bt_actions/test/behavior_tree/FailureNodes.hpp +++ b/plansys2_bt_actions/test/behavior_tree/FailureNodes.hpp @@ -15,28 +15,22 @@ #ifndef BEHAVIOR_TREE__FAILURENODES_HPP_ #define BEHAVIOR_TREE__FAILURENODES_HPP_ -#include #include - -#include "test_msgs/action/fibonacci.hpp" +#include #include "plansys2_bt_actions/BTActionNode.hpp" +#include "test_msgs/action/fibonacci.hpp" namespace plansys2_bt_tests { - class OnTickFail : public plansys2::BtActionNode { public: using Fibonacci = test_msgs::action::Fibonacci; explicit OnTickFail( - const std::string & xml_tag_name, - const std::string & action_name, + const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf) - : plansys2::BtActionNode(xml_tag_name, - action_name, - conf), - on_tick_run(false) + : plansys2::BtActionNode(xml_tag_name, action_name, conf), on_tick_run(false) { } @@ -49,19 +43,14 @@ class OnTickFail : public plansys2::BtActionNode bool on_tick_run; }; - class OnFeedbackFail : public plansys2::BtActionNode { public: using Fibonacci = test_msgs::action::Fibonacci; explicit OnFeedbackFail( - const std::string & xml_tag_name, - const std::string & action_name, + const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf) - : plansys2::BtActionNode(xml_tag_name, - action_name, - conf), - on_feedback_run(false) + : plansys2::BtActionNode(xml_tag_name, action_name, conf), on_feedback_run(false) { } @@ -81,9 +70,9 @@ class OnFeedbackFail : public plansys2::BtActionNode -#include -#include -#include - #include "Move.hpp" -#include "geometry_msgs/msg/pose2_d.hpp" +#include +#include +#include +#include #include "behaviortree_cpp_v3/behavior_tree.h" +#include "geometry_msgs/msg/pose2_d.hpp" namespace plansys2_bt_tests { - Move::Move( - const std::string & xml_tag_name, - const std::string & action_name, + const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf) : plansys2::BtActionNode(xml_tag_name, action_name, conf) { rclcpp::Node::SharedPtr node; config().blackboard->get("node", node); - node->declare_parameter>( - "waypoints", std::vector({})); + node->declare_parameter>("waypoints", std::vector({})); if (node->has_parameter("waypoints")) { std::vector wp_names; @@ -61,8 +57,7 @@ Move::Move( } } -BT::NodeStatus -Move::on_tick() +BT::NodeStatus Move::on_tick() { if (status() == BT::NodeStatus::IDLE) { std::string goal; @@ -84,27 +79,21 @@ Move::on_tick() return BT::NodeStatus::RUNNING; } -BT::NodeStatus -Move::on_success() +BT::NodeStatus Move::on_success() { setOutput("goal_reached", result_.result->sequence[0]); return BT::NodeStatus::SUCCESS; } - } // namespace plansys2_bt_tests #include "behaviortree_cpp_v3/bt_factory.h" BT_REGISTER_NODES(factory) { - BT::NodeBuilder builder = - [](const std::string & name, const BT::NodeConfiguration & config) - { - return std::make_unique( - name, "move", config); - }; - - factory.registerBuilder( - "Move", builder); + BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) { + return std::make_unique(name, "move", config); + }; + + factory.registerBuilder("Move", builder); } diff --git a/plansys2_bt_actions/test/behavior_tree/Move.hpp b/plansys2_bt_actions/test/behavior_tree/Move.hpp index 9f807fbd..491dd848 100644 --- a/plansys2_bt_actions/test/behavior_tree/Move.hpp +++ b/plansys2_bt_actions/test/behavior_tree/Move.hpp @@ -15,25 +15,22 @@ #ifndef BEHAVIOR_TREE__MOVE_HPP_ #define BEHAVIOR_TREE__MOVE_HPP_ -#include #include +#include -#include "geometry_msgs/msg/pose2_d.hpp" -#include "test_msgs/action/fibonacci.hpp" - -#include "plansys2_bt_actions/BTActionNode.hpp" #include "behaviortree_cpp_v3/behavior_tree.h" #include "behaviortree_cpp_v3/bt_factory.h" +#include "geometry_msgs/msg/pose2_d.hpp" +#include "plansys2_bt_actions/BTActionNode.hpp" +#include "test_msgs/action/fibonacci.hpp" namespace plansys2_bt_tests { - class Move : public plansys2::BtActionNode { public: explicit Move( - const std::string & xml_tag_name, - const std::string & action_name, + const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf); BT::NodeStatus on_tick() override; diff --git a/plansys2_bt_actions/test/behavior_tree/OpenGripper.cpp b/plansys2_bt_actions/test/behavior_tree/OpenGripper.cpp index 45b1dd97..c2df44ca 100644 --- a/plansys2_bt_actions/test/behavior_tree/OpenGripper.cpp +++ b/plansys2_bt_actions/test/behavior_tree/OpenGripper.cpp @@ -12,31 +12,23 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include - #include "OpenGripper.hpp" +#include +#include + #include "behaviortree_cpp_v3/behavior_tree.h" namespace plansys2_bt_tests { - -OpenGripper::OpenGripper( - const std::string & xml_tag_name, - const BT::NodeConfiguration & conf) +OpenGripper::OpenGripper(const std::string & xml_tag_name, const BT::NodeConfiguration & conf) : BT::ActionNodeBase(xml_tag_name, conf), counter_(0) { } -void -OpenGripper::halt() -{ - std::cout << "OpenGripper halt" << std::endl; -} +void OpenGripper::halt() { std::cout << "OpenGripper halt" << std::endl; } -BT::NodeStatus -OpenGripper::tick() +BT::NodeStatus OpenGripper::tick() { std::cout << "OpenGripper tick " << counter_ << std::endl; diff --git a/plansys2_bt_actions/test/behavior_tree/OpenGripper.hpp b/plansys2_bt_actions/test/behavior_tree/OpenGripper.hpp index dc64923a..ab9d1740 100644 --- a/plansys2_bt_actions/test/behavior_tree/OpenGripper.hpp +++ b/plansys2_bt_actions/test/behavior_tree/OpenGripper.hpp @@ -22,21 +22,15 @@ namespace plansys2_bt_tests { - class OpenGripper : public BT::ActionNodeBase { public: - explicit OpenGripper( - const std::string & xml_tag_name, - const BT::NodeConfiguration & conf); + explicit OpenGripper(const std::string & xml_tag_name, const BT::NodeConfiguration & conf); void halt(); BT::NodeStatus tick(); - static BT::PortsList providedPorts() - { - return BT::PortsList({}); - } + static BT::PortsList providedPorts() { return BT::PortsList({}); } private: int counter_; diff --git a/plansys2_bt_actions/test/unit/bt_action_test.cpp b/plansys2_bt_actions/test/unit/bt_action_test.cpp index 5bb38656..d187bdfa 100644 --- a/plansys2_bt_actions/test/unit/bt_action_test.cpp +++ b/plansys2_bt_actions/test/unit/bt_action_test.cpp @@ -12,33 +12,27 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include +#include +#include #include #include -#include -#include -#include +#include "../behavior_tree/CloseGripper.hpp" +#include "../behavior_tree/FailureNodes.hpp" +#include "../behavior_tree/Move.hpp" +#include "../behavior_tree/OpenGripper.hpp" +#include "ament_index_cpp/get_package_share_directory.hpp" #include "behaviortree_cpp_v3/behavior_tree.h" #include "behaviortree_cpp_v3/bt_factory.h" #include "behaviortree_cpp_v3/utils/shared_library.h" - -#include "../behavior_tree/OpenGripper.hpp" -#include "../behavior_tree/CloseGripper.hpp" -#include "../behavior_tree/Move.hpp" -#include "../behavior_tree/FailureNodes.hpp" - +#include "gtest/gtest.h" +#include "plansys2_bt_actions/BTAction.hpp" #include "plansys2_executor/ActionExecutor.hpp" - -#include "test_msgs/action/fibonacci.hpp" -#include "ament_index_cpp/get_package_share_directory.hpp" - #include "rclcpp/rclcpp.hpp" -#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp" #include "rclcpp_action/rclcpp_action.hpp" - -#include "plansys2_bt_actions/BTAction.hpp" - -#include "gtest/gtest.h" +#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp" +#include "test_msgs/action/fibonacci.hpp" using namespace std::placeholders; using namespace std::chrono_literals; @@ -49,15 +43,12 @@ class MoveServer : public rclcpp::Node using GoalHandleFibonacci = rclcpp_action::ServerGoalHandle; public: - MoveServer() - : Node("move_server") {} + MoveServer() : Node("move_server") {} void start_server() { move_action_server_ = rclcpp_action::create_server( - shared_from_this(), - "move", - std::bind(&MoveServer::handle_goal, this, _1, _2), + shared_from_this(), "move", std::bind(&MoveServer::handle_goal, this, _1, _2), std::bind(&MoveServer::handle_cancel, this, _1), std::bind(&MoveServer::handle_accepted, this, _1)); } @@ -66,8 +57,7 @@ class MoveServer : public rclcpp::Node rclcpp_action::Server::SharedPtr move_action_server_; rclcpp_action::GoalResponse handle_goal( - const rclcpp_action::GoalUUID & uuid, - std::shared_ptr goal) + const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal) { return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } @@ -103,7 +93,7 @@ class MoveServer : public rclcpp::Node } } - bool cancelled_ {false}; + bool cancelled_{false}; }; TEST(bt_actions, load_plugins) @@ -114,8 +104,10 @@ TEST(bt_actions, load_plugins) bool finish = false; std::thread t([&]() { - while (!finish) {rclcpp::spin_some(move_server_node);} - }); + while (!finish) { + rclcpp::spin_some(move_server_node); + } + }); BT::BehaviorTreeFactory factory; BT::SharedLibrary loader; @@ -151,9 +143,10 @@ TEST(bt_actions, on_tick_failure) bool finished = false; std::thread t([&]() { - while (!finished) {rclcpp::spin_some(move_server_node);} - }); - + while (!finished) { + rclcpp::spin_some(move_server_node); + } + }); BT::NodeConfiguration config; BT::assignDefaultRemapping(config); @@ -188,8 +181,10 @@ TEST(bt_actions, on_feedback_failure) bool finished = false; std::thread t([&]() { - while (!finished) {rclcpp::spin_some(move_server_node);} - }); + while (!finished) { + rclcpp::spin_some(move_server_node); + } + }); BT::NodeConfiguration config; BT::assignDefaultRemapping(config); @@ -197,9 +192,7 @@ TEST(bt_actions, on_feedback_failure) bb->set("node", node); config.blackboard = bb; - plansys2_bt_tests::OnFeedbackFail failure_node("OnFeedbackFail", - "move", - config); + plansys2_bt_tests::OnFeedbackFail failure_node("OnFeedbackFail", "move", config); rclcpp::Rate rate(10); @@ -250,7 +243,7 @@ TEST(bt_actions, bt_action) } auto start = lc_node->now(); - while ( (lc_node->now() - start).seconds() < 2) { + while ((lc_node->now() - start).seconds() < 2) { exe.spin_some(); } } @@ -288,8 +281,10 @@ TEST(bt_actions, cancel_bt_action) bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} - }); + while (!finish) { + exe.spin_some(); + } + }); ASSERT_EQ(action_client->get_internal_status(), plansys2::ActionExecutor::Status::IDLE); ASSERT_EQ( diff --git a/plansys2_core/CMakeLists.txt b/plansys2_core/CMakeLists.txt index 1e6eb605..d8ac5133 100644 --- a/plansys2_core/CMakeLists.txt +++ b/plansys2_core/CMakeLists.txt @@ -38,6 +38,7 @@ install(TARGETS if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) + set(ament_cmake_uncrustify_FOUND TRUE) ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest REQUIRED) diff --git a/plansys2_core/include/plansys2_core/PlanSolverBase.hpp b/plansys2_core/include/plansys2_core/PlanSolverBase.hpp index aad8c601..02696605 100644 --- a/plansys2_core/include/plansys2_core/PlanSolverBase.hpp +++ b/plansys2_core/include/plansys2_core/PlanSolverBase.hpp @@ -15,18 +15,16 @@ #ifndef PLANSYS2_CORE__PLANSOLVERBASE_HPP_ #define PLANSYS2_CORE__PLANSOLVERBASE_HPP_ +#include #include #include -#include #include "plansys2_msgs/msg/plan.hpp" - #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" namespace plansys2 { - class PlanSolverBase { public: diff --git a/plansys2_core/include/plansys2_core/Types.hpp b/plansys2_core/include/plansys2_core/Types.hpp index 5416ceab..7d690f2b 100644 --- a/plansys2_core/include/plansys2_core/Types.hpp +++ b/plansys2_core/include/plansys2_core/Types.hpp @@ -21,59 +21,67 @@ #include "plansys2_msgs/msg/node.hpp" #include "plansys2_msgs/msg/param.hpp" #include "plansys2_msgs/msg/tree.hpp" - #include "plansys2_pddl_parser/Utils.h" namespace plansys2 { - class Instance : public plansys2_msgs::msg::Param { public: - Instance() - : plansys2_msgs::msg::Param() {} + Instance() : plansys2_msgs::msg::Param() {} explicit Instance(const std::string & name, const std::string & type = {}) - : plansys2_msgs::msg::Param(parser::pddl::fromStringParam(name, type)) {} + : plansys2_msgs::msg::Param(parser::pddl::fromStringParam(name, type)) + { + } Instance(const plansys2_msgs::msg::Param & instance) // NOLINT(runtime/explicit) - : plansys2_msgs::msg::Param(instance) {} + : plansys2_msgs::msg::Param(instance) + { + } }; class Predicate : public plansys2_msgs::msg::Node { public: - Predicate() - : plansys2_msgs::msg::Node() {} + Predicate() : plansys2_msgs::msg::Node() {} explicit Predicate(const std::string & pred) - : plansys2_msgs::msg::Node(parser::pddl::fromStringPredicate(pred)) {} + : plansys2_msgs::msg::Node(parser::pddl::fromStringPredicate(pred)) + { + } Predicate(const plansys2_msgs::msg::Node & pred) // NOLINT(runtime/explicit) - : plansys2_msgs::msg::Node(pred) {} + : plansys2_msgs::msg::Node(pred) + { + } }; class Function : public plansys2_msgs::msg::Node { public: - Function() - : plansys2_msgs::msg::Node() {} + Function() : plansys2_msgs::msg::Node() {} explicit Function(const std::string & func) - : plansys2_msgs::msg::Node(parser::pddl::fromStringFunction(func)) {} + : plansys2_msgs::msg::Node(parser::pddl::fromStringFunction(func)) + { + } Function(const plansys2_msgs::msg::Node & func) // NOLINT(runtime/explicit) - : plansys2_msgs::msg::Node(func) {} + : plansys2_msgs::msg::Node(func) + { + } }; class Goal : public plansys2_msgs::msg::Tree { public: - Goal() - : plansys2_msgs::msg::Tree() {} - explicit Goal(const std::string & goal) - : plansys2_msgs::msg::Tree(parser::pddl::fromString(goal)) {} + Goal() : plansys2_msgs::msg::Tree() {} + explicit Goal(const std::string & goal) : plansys2_msgs::msg::Tree(parser::pddl::fromString(goal)) + { + } Goal(const plansys2_msgs::msg::Tree & goal) // NOLINT(runtime/explicit) - : plansys2_msgs::msg::Tree(goal) {} + : plansys2_msgs::msg::Tree(goal) + { + } }; -template -std::vector -convertVector(const std::vector & in_vector) +template +std::vector convertVector(const std::vector & in_vector) { std::vector ret; for (const auto & item : in_vector) { diff --git a/plansys2_core/include/plansys2_core/Utils.hpp b/plansys2_core/include/plansys2_core/Utils.hpp index 6c1242d2..8ea32b3b 100644 --- a/plansys2_core/include/plansys2_core/Utils.hpp +++ b/plansys2_core/include/plansys2_core/Utils.hpp @@ -20,7 +20,6 @@ namespace plansys2 { - std::vector tokenize(const std::string & string, const std::string & delim); /** @@ -32,9 +31,7 @@ std::vector tokenize(const std::string & string, const std::string * @return a substring without empty lines */ std::string substr_without_empty_lines( - std::string string, - std::size_t init_pos, - std::size_t end_pos); + std::string string, std::size_t init_pos, std::size_t end_pos); /** * @brief remove the comments on a pddl string diff --git a/plansys2_core/package.xml b/plansys2_core/package.xml index 0c08612e..943212b2 100644 --- a/plansys2_core/package.xml +++ b/plansys2_core/package.xml @@ -3,11 +3,11 @@ plansys2_core 2.0.8 - + This package contains the PDDL-based core for the ROS2 Planning System Francisco Martin Rico - + Apache License, Version 2.0 ament_cmake diff --git a/plansys2_core/src/plansys2_core/Utils.cpp b/plansys2_core/src/plansys2_core/Utils.cpp index 38c83236..42ca770a 100644 --- a/plansys2_core/src/plansys2_core/Utils.cpp +++ b/plansys2_core/src/plansys2_core/Utils.cpp @@ -12,15 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "plansys2_core/Utils.hpp" + #include +#include #include -#include "plansys2_core/Utils.hpp" - namespace plansys2 { - std::vector tokenize(const std::string & string, const std::string & delim) { std::string::size_type lastPos = 0, pos = string.find_first_of(delim, lastPos); @@ -41,9 +40,7 @@ std::vector tokenize(const std::string & string, const std::string } std::string substr_without_empty_lines( - std::string string, - std::size_t init_pos, - std::size_t end_pos) + std::string string, std::size_t init_pos, std::size_t end_pos) { std::stringstream stream_in(string.substr(init_pos, end_pos - init_pos)); std::stringstream stream_out; diff --git a/plansys2_core/test/utils_test.cpp b/plansys2_core/test/utils_test.cpp index e2251ffb..b329af4b 100644 --- a/plansys2_core/test/utils_test.cpp +++ b/plansys2_core/test/utils_test.cpp @@ -15,9 +15,9 @@ #include #include -#include "gtest/gtest.h" #include "plansys2_core/Utils.hpp" +#include "gtest/gtest.h" TEST(domain_expert, functions) { diff --git a/plansys2_docs/tutorials/tut_1_terminal.md b/plansys2_docs/tutorials/tut_1_terminal.md index 11dd4f09..77ad2f6c 100644 --- a/plansys2_docs/tutorials/tut_1_terminal.md +++ b/plansys2_docs/tutorials/tut_1_terminal.md @@ -91,7 +91,7 @@ And get the plan. This command will not execute the plan. Only will calculate it ``` plansys2_terminal > get plan -plan: +plan: 0 (askcharge leia entrance chargingroom) 5 0.001 (charge leia chargingroom) 5 5.002 (move leia chargingroom kitchen) 5 diff --git a/plansys2_docs/tutorials/tut_2_patrol.md b/plansys2_docs/tutorials/tut_2_patrol.md index ebc46323..81c6a5fb 100644 --- a/plansys2_docs/tutorials/tut_2_patrol.md +++ b/plansys2_docs/tutorials/tut_2_patrol.md @@ -8,7 +8,7 @@ In one terminal run: ros2 launch patrol_navigation_example patrol_example_launch.py ``` -This command launchs plansys2 and the nodes that implements the actions (move and patrol). The navigation param `autostart` is set to `true`, and the amcl is set to the starting position of the robot. This position is `wp_control`. We will use the next waypoints (x, y, yaw): +This command launches plansys2 and the nodes that implements the actions (move and patrol). The navigation param `autostart` is set to `true`, and the amcl is set to the starting position of the robot. This position is `wp_control`. We will use the next waypoints (x, y, yaw): - wp_control (-2.0, -4.0, 0.0) - This is the starting point - wp1 (0.0, -2.0, 0.0) diff --git a/plansys2_domain_expert/CHANGELOG.rst b/plansys2_domain_expert/CHANGELOG.rst index 052aa728..0aca8013 100644 --- a/plansys2_domain_expert/CHANGELOG.rst +++ b/plansys2_domain_expert/CHANGELOG.rst @@ -170,7 +170,7 @@ Changelog for package plansys2_domain_expert Signed-off-by: Francisco Martin Rico * Setting CI Signed-off-by: Francisco Martin Rico -* Change to lowercasegit +* Change to lowercasegit Signed-off-by: Francisco Martin Rico * First version of planner complete Signed-off-by: Francisco Martin Rico diff --git a/plansys2_domain_expert/CMakeLists.txt b/plansys2_domain_expert/CMakeLists.txt index 006e2e8d..3fc75e14 100644 --- a/plansys2_domain_expert/CMakeLists.txt +++ b/plansys2_domain_expert/CMakeLists.txt @@ -63,6 +63,7 @@ install(TARGETS if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) + set(ament_cmake_uncrustify_FOUND TRUE) ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest REQUIRED) diff --git a/plansys2_domain_expert/README.md b/plansys2_domain_expert/README.md index 753ee12c..930b697a 100644 --- a/plansys2_domain_expert/README.md +++ b/plansys2_domain_expert/README.md @@ -1,6 +1,6 @@ # Domain Expert -The Domain Expert module is responsible for maintaining the PDDL domain. +The Domain Expert module is responsible for maintaining the PDDL domain. The main class is [`plansys2::DomainExpertNode`](include/plansys2_domain_expert/DomainExpertNode.hpp), which is instantiated from [`DomainExpertNode.cpp`](src/DomainExpertNode.cpp). `plansys2::DomainExpertNode` is a `rclcpp_lifecycle::LifecycleNode` and in its configuration phase reads the `model_file` parameter, which contains the .pddl file from which to read the model. diff --git a/plansys2_domain_expert/include/plansys2_domain_expert/DomainExpert.hpp b/plansys2_domain_expert/include/plansys2_domain_expert/DomainExpert.hpp index 51feb5a9..2d782e29 100644 --- a/plansys2_domain_expert/include/plansys2_domain_expert/DomainExpert.hpp +++ b/plansys2_domain_expert/include/plansys2_domain_expert/DomainExpert.hpp @@ -15,23 +15,20 @@ #ifndef PLANSYS2_DOMAIN_EXPERT__DOMAINEXPERT_HPP_ #define PLANSYS2_DOMAIN_EXPERT__DOMAINEXPERT_HPP_ +#include #include #include #include -#include +#include "plansys2_domain_expert/DomainExpertInterface.hpp" +#include "plansys2_domain_expert/DomainReader.hpp" #include "plansys2_msgs/msg/action.hpp" #include "plansys2_msgs/msg/durative_action.hpp" #include "plansys2_msgs/msg/node.hpp" - #include "plansys2_pddl_parser/Domain.h" -#include "plansys2_domain_expert/DomainExpertInterface.hpp" -#include "plansys2_domain_expert/DomainReader.hpp" - namespace plansys2 { - /// DomainExpert is in charge of managing the internal structure of a domain. class DomainExpert : public DomainExpertInterface { @@ -76,8 +73,9 @@ class DomainExpert : public DomainExpertInterface /// Get the details of a predicate existing in the domain. /** * \param[in] predicate The name of the predicate. - * \return A Predicate object containing the predicate name and its parameters (name and type). - * If the predicate does not exist, the value returned has not value. + * \return A Predicate object containing the predicate name and its parameters + * (name and type). If the predicate does not exist, the value returned has + * not value. */ std::optional getPredicate(const std::string & predicate); @@ -90,8 +88,9 @@ class DomainExpert : public DomainExpertInterface /// Get the details of a function existing in the domain. /** * \param[in] function The name of the function. - * \return A Function object containing the function name and its parameters (name and type). - * If the function does not exist, the value returned has not value. + * \return A Function object containing the function name and its parameters + * (name and type). If the function does not exist, the value returned has not + * value. */ std::optional getFunction(const std::string & function); @@ -104,12 +103,12 @@ class DomainExpert : public DomainExpertInterface /// Get the details of an regular action existing in the domain. /** * \param[in] action The name of the action. - * \return An Action object containing the action name, parameters, requirements and effects. - * If the action does not exist, the value returned has not value. + * \return An Action object containing the action name, parameters, + * requirements and effects. If the action does not exist, the value returned + * has not value. */ plansys2_msgs::msg::Action::SharedPtr getAction( - const std::string & action, - const std::vector & params = {}); + const std::string & action, const std::vector & params = {}); /// Get the temporal actions existing in the domain. /** @@ -120,14 +119,15 @@ class DomainExpert : public DomainExpertInterface /// Get the details of an durative action existing in the domain. /** * \param[in] action The name of the action. - * \return A Durative Action object containing the action name, parameters, requirements and - * effects. If the action does not exist, the value returned has not value. + * \return A Durative Action object containing the action name, parameters, + * requirements and effects. If the action does not exist, the value returned + * has not value. */ plansys2_msgs::msg::DurativeAction::SharedPtr getDurativeAction( - const std::string & action, - const std::vector & params = {}); + const std::string & action, const std::vector & params = {}); - /// Get the current domain, ready to be saved to file, or to initialize another domain. + /// Get the current domain, ready to be saved to file, or to initialize + /// another domain. /** * \return A string containing the domain. */ diff --git a/plansys2_domain_expert/include/plansys2_domain_expert/DomainExpertClient.hpp b/plansys2_domain_expert/include/plansys2_domain_expert/DomainExpertClient.hpp index 79565cff..84d56ddc 100644 --- a/plansys2_domain_expert/include/plansys2_domain_expert/DomainExpertClient.hpp +++ b/plansys2_domain_expert/include/plansys2_domain_expert/DomainExpertClient.hpp @@ -15,38 +15,35 @@ #ifndef PLANSYS2_DOMAIN_EXPERT__DOMAINEXPERTCLIENT_HPP_ #define PLANSYS2_DOMAIN_EXPERT__DOMAINEXPERTCLIENT_HPP_ +#include #include #include #include -#include #include "plansys2_core/Types.hpp" #include "plansys2_domain_expert/DomainExpertInterface.hpp" - #include "plansys2_msgs/msg/action.hpp" #include "plansys2_msgs/msg/durative_action.hpp" #include "plansys2_msgs/msg/node.hpp" - #include "plansys2_msgs/srv/get_domain.hpp" -#include "plansys2_msgs/srv/get_domain_name.hpp" -#include "plansys2_msgs/srv/get_domain_types.hpp" -#include "plansys2_msgs/srv/get_domain_constants.hpp" -#include "plansys2_msgs/srv/get_domain_actions.hpp" #include "plansys2_msgs/srv/get_domain_action_details.hpp" +#include "plansys2_msgs/srv/get_domain_actions.hpp" +#include "plansys2_msgs/srv/get_domain_constants.hpp" #include "plansys2_msgs/srv/get_domain_durative_action_details.hpp" +#include "plansys2_msgs/srv/get_domain_name.hpp" +#include "plansys2_msgs/srv/get_domain_types.hpp" #include "plansys2_msgs/srv/get_node_details.hpp" #include "plansys2_msgs/srv/get_states.hpp" - #include "rclcpp/rclcpp.hpp" namespace plansys2 { - -/// DomainExpertClient requests changes or gets information to/from the DomainExpertNode +/// DomainExpertClient requests changes or gets information to/from the +/// DomainExpertNode /** * Any node can create a DomainExpertClient object to requests changes to the - * DomainExpertNode, or to get information from it. It presents the same interface - * of the DomainExpert, and hides the complexity of using services. + * DomainExpertNode, or to get information from it. It presents the same + * interface of the DomainExpert, and hides the complexity of using services. */ class DomainExpertClient : public DomainExpertInterface { @@ -82,8 +79,9 @@ class DomainExpertClient : public DomainExpertInterface /// Get the details of a predicate existing in the domain. /** * \param[in] predicate The name of the predicate. - * \return A Predicate object containing the predicate name and its parameters (name and type). - * If the predicate does not exist, the value returned has not value. + * \return A Predicate object containing the predicate name and its parameters + * (name and type). If the predicate does not exist, the value returned has + * not value. */ std::optional getPredicate(const std::string & predicate); @@ -96,8 +94,9 @@ class DomainExpertClient : public DomainExpertInterface /// Get the details of a function existing in the domain. /** * \param[in] function The name of the function. - * \return A Function object containing the function name and its parameters (name and type). - * If the function does not exist, the value returned has not value. + * \return A Function object containing the function name and its parameters + * (name and type). If the function does not exist, the value returned has not + * value. */ std::optional getFunction(const std::string & function); @@ -110,12 +109,12 @@ class DomainExpertClient : public DomainExpertInterface /// Get the details of a regular action existing in the domain. /** * \param[in] action The name of the action. - * \return An Action object containing the action name, parameters, requirements and effects. - * If the action does not exist, the value returned has not value. + * \return An Action object containing the action name, parameters, + * requirements and effects. If the action does not exist, the value returned + * has not value. */ plansys2_msgs::msg::Action::SharedPtr getAction( - const std::string & action, - const std::vector & params = {}); + const std::string & action, const std::vector & params = {}); /// Get the temporal actions existing in the domain. /** @@ -126,14 +125,15 @@ class DomainExpertClient : public DomainExpertInterface /// Get the details of a durative action existing in the domain. /** * \param[in] action The name of the action. - * \return A Durative Action object containing the action name, parameters, requirements and - * effects. If the action does not exist, the value returned has not value. + * \return A Durative Action object containing the action name, parameters, + * requirements and effects. If the action does not exist, the value returned + * has not value. */ plansys2_msgs::msg::DurativeAction::SharedPtr getDurativeAction( - const std::string & action, - const std::vector & params = {}); + const std::string & action, const std::vector & params = {}); - /// Get the current domain, ready to be saved to file, or to initialize another domain. + /// Get the current domain, ready to be saved to file, or to initialize + /// another domain. /** * \return A string containing the domain. */ diff --git a/plansys2_domain_expert/include/plansys2_domain_expert/DomainExpertInterface.hpp b/plansys2_domain_expert/include/plansys2_domain_expert/DomainExpertInterface.hpp index 9a2312fc..228142a7 100644 --- a/plansys2_domain_expert/include/plansys2_domain_expert/DomainExpertInterface.hpp +++ b/plansys2_domain_expert/include/plansys2_domain_expert/DomainExpertInterface.hpp @@ -19,16 +19,15 @@ #include #include +#include "plansys2_core/Types.hpp" #include "plansys2_msgs/msg/action.hpp" #include "plansys2_msgs/msg/durative_action.hpp" #include "plansys2_msgs/msg/node.hpp" -#include "plansys2_core/Types.hpp" - namespace plansys2 { - -/// DomainExpertInterface is the interface for both DomainExpert and DomainExpertClient +/// DomainExpertInterface is the interface for both DomainExpert and +/// DomainExpertClient class DomainExpertInterface { public: @@ -65,11 +64,11 @@ class DomainExpertInterface /// Get the details of a predicate existing in the domain. /** * \param[in] predicate The name of the predicate. - * \return A Predicate object containing the predicate name and its parameters (name and type). - * If the predicate does not exist, the value returned has not value. + * \return A Predicate object containing the predicate name and its parameters + * (name and type). If the predicate does not exist, the value returned has + * not value. */ - virtual std::optional getPredicate(const std::string & predicate) - = 0; + virtual std::optional getPredicate(const std::string & predicate) = 0; /// Get the functions existing in the domain. /** @@ -80,11 +79,11 @@ class DomainExpertInterface /// Get the details of a function existing in the domain. /** * \param[in] function The name of the function. - * \return A Function object containing the function name and its parameters (name and type). - * If the function does not exist, the value returned has not value. + * \return A Function object containing the function name and its parameters + * (name and type). If the function does not exist, the value returned has not + * value. */ - virtual std::optional getFunction(const std::string & function) = - 0; + virtual std::optional getFunction(const std::string & function) = 0; /// Get the regular actions existing in the domain. /** @@ -95,12 +94,12 @@ class DomainExpertInterface /// Get the details of an regular action existing in the domain. /** * \param[in] action The name of the action. - * \return An Action object containing the action name, parameters, requirements and effects. - * If the action does not exist, the value returned has not value. + * \return An Action object containing the action name, parameters, + * requirements and effects. If the action does not exist, the value returned + * has not value. */ virtual plansys2_msgs::msg::Action::SharedPtr getAction( - const std::string & action, const std::vector & params) = - 0; + const std::string & action, const std::vector & params) = 0; /// Get the temporal actions existing in the domain. /** @@ -111,14 +110,15 @@ class DomainExpertInterface /// Get the details of an durative action existing in the domain. /** * \param[in] action The name of the action. - * \return A Durative Action object containing the action name, parameters, requirements and - * effects. If the action does not exist, the value returned has not value. + * \return A Durative Action object containing the action name, parameters, + * requirements and effects. If the action does not exist, the value returned + * has not value. */ virtual plansys2_msgs::msg::DurativeAction::SharedPtr getDurativeAction( - const std::string & durative_action, const std::vector & params) = - 0; + const std::string & durative_action, const std::vector & params) = 0; - /// Get the current domain, ready to be saved to file, or to initialize another domain. + /// Get the current domain, ready to be saved to file, or to initialize + /// another domain. /** * \return A string containing the domain. */ diff --git a/plansys2_domain_expert/include/plansys2_domain_expert/DomainExpertNode.hpp b/plansys2_domain_expert/include/plansys2_domain_expert/DomainExpertNode.hpp index e85e6139..1a87266d 100644 --- a/plansys2_domain_expert/include/plansys2_domain_expert/DomainExpertNode.hpp +++ b/plansys2_domain_expert/include/plansys2_domain_expert/DomainExpertNode.hpp @@ -15,38 +15,35 @@ #ifndef PLANSYS2_DOMAIN_EXPERT__DOMAINEXPERTNODE_HPP_ #define PLANSYS2_DOMAIN_EXPERT__DOMAINEXPERTNODE_HPP_ -#include #include +#include -#include "plansys2_domain_expert/DomainExpert.hpp" - -#include "std_msgs/msg/string.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "lifecycle_msgs/msg/transition.hpp" -#include "plansys2_msgs/srv/get_domain_name.hpp" -#include "plansys2_msgs/srv/get_domain_types.hpp" -#include "plansys2_msgs/srv/get_domain_actions.hpp" +#include "plansys2_domain_expert/DomainExpert.hpp" +#include "plansys2_msgs/srv/get_domain.hpp" #include "plansys2_msgs/srv/get_domain_action_details.hpp" +#include "plansys2_msgs/srv/get_domain_actions.hpp" #include "plansys2_msgs/srv/get_domain_durative_action_details.hpp" -#include "plansys2_msgs/srv/get_domain.hpp" +#include "plansys2_msgs/srv/get_domain_name.hpp" +#include "plansys2_msgs/srv/get_domain_types.hpp" #include "plansys2_msgs/srv/get_node_details.hpp" #include "plansys2_msgs/srv/get_states.hpp" - #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "std_msgs/msg/string.hpp" namespace plansys2 { - -/// DomainExpertNode contains a model, and manages the requests from the DomainExpertClient. +/// DomainExpertNode contains a model, and manages the requests from the +/// DomainExpertClient. class DomainExpertNode : public rclcpp_lifecycle::LifecycleNode { public: /// Create a new DomainExpertNode DomainExpertNode(); - using CallbackReturnT = - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + using CallbackReturnT = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; /// Configures domain by creating a DomainExpert object /** @@ -90,7 +87,6 @@ class DomainExpertNode : public rclcpp_lifecycle::LifecycleNode */ CallbackReturnT on_error(const rclcpp_lifecycle::State & state); - /// Receives the result of the GetDomainName service call /** * \param[in] request_header The header of the request @@ -224,12 +220,10 @@ class DomainExpertNode : public rclcpp_lifecycle::LifecycleNode get_domain_durative_actions_service_; rclcpp::Service::SharedPtr get_domain_durative_action_details_service_; - rclcpp::Service::SharedPtr - get_domain_predicates_service_; + rclcpp::Service::SharedPtr get_domain_predicates_service_; rclcpp::Service::SharedPtr get_domain_predicate_details_service_; - rclcpp::Service::SharedPtr - get_domain_functions_service_; + rclcpp::Service::SharedPtr get_domain_functions_service_; rclcpp::Service::SharedPtr get_domain_function_details_service_; rclcpp::Service::SharedPtr get_domain_service_; diff --git a/plansys2_domain_expert/include/plansys2_domain_expert/DomainReader.hpp b/plansys2_domain_expert/include/plansys2_domain_expert/DomainReader.hpp index a0f5844c..5caafde0 100644 --- a/plansys2_domain_expert/include/plansys2_domain_expert/DomainReader.hpp +++ b/plansys2_domain_expert/include/plansys2_domain_expert/DomainReader.hpp @@ -20,7 +20,6 @@ namespace plansys2 { - struct Domain { std::string name; @@ -39,7 +38,7 @@ class DomainReader void add_domain(const std::string & domain); std::string get_joint_domain() const; - std::vector get_domains() {return domains_;} + std::vector get_domains() { return domains_; } protected: int get_end_block(const std::string & domain, std::size_t init_pos); diff --git a/plansys2_domain_expert/launch/domain_expert_launch.py b/plansys2_domain_expert/launch/domain_expert_launch.py index 2ecacfdd..07785020 100644 --- a/plansys2_domain_expert/launch/domain_expert_launch.py +++ b/plansys2_domain_expert/launch/domain_expert_launch.py @@ -1,3 +1,4 @@ +# -*- coding: utf-8 -*- # Copyright 2019 Intelligent Robotics Lab # # Licensed under the Apache License, Version 2.0 (the "License"); @@ -20,30 +21,30 @@ def generate_launch_description(): # Create the launch configuration variables - model_file = LaunchConfiguration('model_file') - namespace = LaunchConfiguration('namespace') - params_file = LaunchConfiguration('params_file') + model_file = LaunchConfiguration("model_file") + namespace = LaunchConfiguration("namespace") + params_file = LaunchConfiguration("params_file") declare_model_file_cmd = DeclareLaunchArgument( - 'model_file', - default_value='src/ros2_planning_system/' - 'plansys2_domain_expert/test/pddl/domain_simple.pddl', - description='PDDL Model file') + "model_file", + default_value="src/ros2_planning_system/" + "plansys2_domain_expert/test/pddl/domain_simple.pddl", + description="PDDL Model file", + ) declare_namespace_cmd = DeclareLaunchArgument( - 'namespace', - default_value='', - description='Namespace') + "namespace", default_value="", description="Namespace" + ) # Specify the actions domain_expert_cmd = Node( - package='plansys2_domain_expert', - executable='domain_expert_node', - name='domain_expert', + package="plansys2_domain_expert", + executable="domain_expert_node", + name="domain_expert", namespace=namespace, - output='screen', - parameters=[ - {'model_file': model_file}, params_file]) + output="screen", + parameters=[{"model_file": model_file}, params_file], + ) # Create the launch description and populate ld = LaunchDescription() diff --git a/plansys2_domain_expert/package.xml b/plansys2_domain_expert/package.xml index 1bef7a18..25e4fb84 100644 --- a/plansys2_domain_expert/package.xml +++ b/plansys2_domain_expert/package.xml @@ -3,11 +3,11 @@ plansys2_domain_expert 2.0.8 - + This package contains the Domain Expert module for the ROS2 Planning System Francisco Martin Rico - + Apache License, Version 2.0 ament_cmake @@ -22,7 +22,7 @@ plansys2_popf_plan_solver std_msgs lifecycle_msgs - + ament_lint_common ament_lint_auto ament_cmake_gtest diff --git a/plansys2_domain_expert/src/plansys2_domain_expert/DomainExpert.cpp b/plansys2_domain_expert/src/plansys2_domain_expert/DomainExpert.cpp index 7fcc05e2..4fc862f1 100644 --- a/plansys2_domain_expert/src/plansys2_domain_expert/DomainExpert.cpp +++ b/plansys2_domain_expert/src/plansys2_domain_expert/DomainExpert.cpp @@ -14,23 +14,17 @@ #include "plansys2_domain_expert/DomainExpert.hpp" -#include #include +#include +#include #include #include -#include - namespace plansys2 { +DomainExpert::DomainExpert(const std::string & domain) { extendDomain(domain); } -DomainExpert::DomainExpert(const std::string & domain) -{ - extendDomain(domain); -} - -void -DomainExpert::extendDomain(const std::string & domain) +void DomainExpert::extendDomain(const std::string & domain) { domains_.add_domain(domain); @@ -44,14 +38,9 @@ DomainExpert::extendDomain(const std::string & domain) } } -std::string -DomainExpert::getName() -{ - return domain_->name; -} +std::string DomainExpert::getName() { return domain_->name; } -std::vector -DomainExpert::getTypes() +std::vector DomainExpert::getTypes() { std::vector ret; if (domain_->typed) { @@ -62,16 +51,16 @@ DomainExpert::getTypes() return ret; } -std::vector -DomainExpert::getConstants(const std::string & type) +std::vector DomainExpert::getConstants(const std::string & type) { - if (!domain_->typed) {return std::vector();} + if (!domain_->typed) { + return std::vector(); + } return domain_->getType(type)->constants.tokens; } -std::vector -DomainExpert::getPredicates() +std::vector DomainExpert::getPredicates() { std::vector ret; for (unsigned i = 0; i < domain_->preds.size(); i++) { @@ -83,13 +72,11 @@ DomainExpert::getPredicates() return ret; } -std::optional -DomainExpert::getPredicate(const std::string & predicate) +std::optional DomainExpert::getPredicate(const std::string & predicate) { std::string predicate_search = predicate; std::transform( - predicate_search.begin(), predicate_search.end(), - predicate_search.begin(), ::tolower); + predicate_search.begin(), predicate_search.end(), predicate_search.begin(), ::tolower); plansys2_msgs::msg::Node ret; bool found = false; @@ -101,8 +88,8 @@ DomainExpert::getPredicate(const std::string & predicate) ret.name = predicate_search; for (unsigned j = 0; j < domain_->preds[i]->params.size(); j++) { plansys2_msgs::msg::Param param; - param.name = "?" + domain_->types[domain_->preds[i]->params[j]]->getName() + - std::to_string(j); + param.name = + "?" + domain_->types[domain_->preds[i]->params[j]]->getName() + std::to_string(j); param.type = domain_->types[domain_->preds[i]->params[j]]->name; domain_->types[domain_->preds[i]->params[j]]->getSubTypesNames(param.sub_types); ret.parameters.push_back(param); @@ -118,8 +105,7 @@ DomainExpert::getPredicate(const std::string & predicate) } } -std::vector -DomainExpert::getFunctions() +std::vector DomainExpert::getFunctions() { std::vector ret; for (unsigned i = 0; i < domain_->funcs.size(); i++) { @@ -131,13 +117,11 @@ DomainExpert::getFunctions() return ret; } -std::optional -DomainExpert::getFunction(const std::string & function) +std::optional DomainExpert::getFunction(const std::string & function) { std::string function_search = function; std::transform( - function_search.begin(), function_search.end(), - function_search.begin(), ::tolower); + function_search.begin(), function_search.end(), function_search.begin(), ::tolower); plansys2::Function ret; bool found = false; @@ -149,8 +133,8 @@ DomainExpert::getFunction(const std::string & function) ret.name = function_search; for (unsigned j = 0; j < domain_->funcs[i]->params.size(); j++) { plansys2_msgs::msg::Param param; - param.name = "?" + domain_->types[domain_->funcs[i]->params[j]]->getName() + - std::to_string(j); + param.name = + "?" + domain_->types[domain_->funcs[i]->params[j]]->getName() + std::to_string(j); param.type = domain_->types[domain_->funcs[i]->params[j]]->name; domain_->types[domain_->funcs[i]->params[j]]->getSubTypesNames(param.sub_types); ret.parameters.push_back(param); @@ -166,8 +150,7 @@ DomainExpert::getFunction(const std::string & function) } } -std::vector -DomainExpert::getActions() +std::vector DomainExpert::getActions() { std::vector ret; for (unsigned i = 0; i < domain_->actions.size(); i++) { @@ -180,13 +163,11 @@ DomainExpert::getActions() return ret; } -plansys2_msgs::msg::Action::SharedPtr -DomainExpert::getAction(const std::string & action, const std::vector & params) +plansys2_msgs::msg::Action::SharedPtr DomainExpert::getAction( + const std::string & action, const std::vector & params) { std::string action_search = action; - std::transform( - action_search.begin(), action_search.end(), - action_search.begin(), ::tolower); + std::transform(action_search.begin(), action_search.end(), action_search.begin(), ::tolower); auto ret = std::make_shared(); bool found = false; @@ -233,8 +214,7 @@ DomainExpert::getAction(const std::string & action, const std::vector -DomainExpert::getDurativeActions() +std::vector DomainExpert::getDurativeActions() { std::vector ret; for (unsigned i = 0; i < domain_->actions.size(); i++) { @@ -249,13 +229,11 @@ DomainExpert::getDurativeActions() return ret; } -plansys2_msgs::msg::DurativeAction::SharedPtr -DomainExpert::getDurativeAction(const std::string & action, const std::vector & params) +plansys2_msgs::msg::DurativeAction::SharedPtr DomainExpert::getDurativeAction( + const std::string & action, const std::vector & params) { std::string action_search = action; - std::transform( - action_search.begin(), action_search.end(), - action_search.begin(), ::tolower); + std::transform(action_search.begin(), action_search.end(), action_search.begin(), ::tolower); auto ret = std::make_shared(); bool found = false; @@ -319,14 +297,9 @@ DomainExpert::getDurativeAction(const std::string & action, const std::vector #include +#include +#include #include #include -#include namespace plansys2 { - DomainExpertClient::DomainExpertClient() { node_ = rclcpp::Node::make_shared("domain_expert_client"); - get_domain_client_ = node_->create_client( - "domain_expert/get_domain"); - get_name_client_ = node_->create_client( - "domain_expert/get_domain_name"); - get_types_client_ = node_->create_client( - "domain_expert/get_domain_types"); + get_domain_client_ = + node_->create_client("domain_expert/get_domain"); + get_name_client_ = + node_->create_client("domain_expert/get_domain_name"); + get_types_client_ = + node_->create_client("domain_expert/get_domain_types"); get_constants_client_ = node_->create_client( "domain_expert/get_domain_constants"); - get_predicates_client_ = node_->create_client( - "domain_expert/get_domain_predicates"); - get_functions_client_ = node_->create_client( - "domain_expert/get_domain_functions"); - get_actions_client_ = node_->create_client( - "domain_expert/get_domain_actions"); + get_predicates_client_ = + node_->create_client("domain_expert/get_domain_predicates"); + get_functions_client_ = + node_->create_client("domain_expert/get_domain_functions"); + get_actions_client_ = + node_->create_client("domain_expert/get_domain_actions"); get_durative_actions_client_ = node_->create_client( "domain_expert/get_domain_durative_actions"); - get_predicate_details_client_ = - node_->create_client( + get_predicate_details_client_ = node_->create_client( "domain_expert/get_domain_predicate_details"); - get_function_details_client_ = - node_->create_client( + get_function_details_client_ = node_->create_client( "domain_expert/get_domain_function_details"); get_action_details_client_ = node_->create_client( "domain_expert/get_domain_action_details"); get_durative_action_details_client_ = node_->create_client( - "domain_expert/get_domain_durative_action_details"); + "domain_expert/get_domain_durative_action_details"); } -std::string -DomainExpertClient::getName() +std::string DomainExpertClient::getName() { std::string ret; @@ -66,18 +62,17 @@ DomainExpertClient::getName() return ret; } RCLCPP_INFO_STREAM( - node_->get_logger(), - get_name_client_->get_service_name() << - " service client: waiting for service to appear..."); + node_->get_logger(), get_name_client_->get_service_name() << " service client: waiting " + "for service to appear..."); } auto request = std::make_shared(); auto future_result = get_name_client_->async_send_request(request); - if (rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != - rclcpp::FutureReturnCode::SUCCESS) - { + if ( + rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != + rclcpp::FutureReturnCode::SUCCESS) { return ret; } @@ -88,8 +83,7 @@ DomainExpertClient::getName() return ret; } -std::vector -DomainExpertClient::getTypes() +std::vector DomainExpertClient::getTypes() { std::vector ret; @@ -98,18 +92,17 @@ DomainExpertClient::getTypes() return ret; } RCLCPP_ERROR_STREAM( - node_->get_logger(), - get_types_client_->get_service_name() << - " service client: waiting for service to appear..."); + node_->get_logger(), get_types_client_->get_service_name() << " service client: waiting " + "for service to appear..."); } auto request = std::make_shared(); auto future_result = get_types_client_->async_send_request(request); - if (rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != - rclcpp::FutureReturnCode::SUCCESS) - { + if ( + rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != + rclcpp::FutureReturnCode::SUCCESS) { return ret; } @@ -120,8 +113,7 @@ DomainExpertClient::getTypes() return ret; } -std::vector -DomainExpertClient::getConstants(const std::string & type) +std::vector DomainExpertClient::getConstants(const std::string & type) { std::vector ret; @@ -130,18 +122,17 @@ DomainExpertClient::getConstants(const std::string & type) return ret; } RCLCPP_ERROR_STREAM( - node_->get_logger(), - get_constants_client_->get_service_name() << - " service client: waiting for service to appear..."); + node_->get_logger(), get_constants_client_->get_service_name() + << " service client: waiting for service to appear..."); } auto request = std::make_shared(); auto future_result = get_constants_client_->async_send_request(request); - if (rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != - rclcpp::FutureReturnCode::SUCCESS) - { + if ( + rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != + rclcpp::FutureReturnCode::SUCCESS) { return ret; } @@ -152,8 +143,7 @@ DomainExpertClient::getConstants(const std::string & type) return ret; } -std::vector -DomainExpertClient::getPredicates() +std::vector DomainExpertClient::getPredicates() { std::vector ret; @@ -162,40 +152,36 @@ DomainExpertClient::getPredicates() return ret; } RCLCPP_ERROR_STREAM( - node_->get_logger(), - get_predicates_client_->get_service_name() << - " service client: waiting for service to appear..."); + node_->get_logger(), get_predicates_client_->get_service_name() + << " service client: waiting for service to appear..."); } auto request = std::make_shared(); auto future_result = get_predicates_client_->async_send_request(request); - if (rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != - rclcpp::FutureReturnCode::SUCCESS) - { + if ( + rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != + rclcpp::FutureReturnCode::SUCCESS) { return ret; } auto result = *future_result.get(); - ret = plansys2::convertVector( - result.states); + ret = plansys2::convertVector(result.states); return ret; } -std::optional -DomainExpertClient::getPredicate(const std::string & predicate) +std::optional DomainExpertClient::getPredicate(const std::string & predicate) { while (!get_predicate_details_client_->wait_for_service(std::chrono::seconds(1))) { if (!rclcpp::ok()) { return {}; } RCLCPP_ERROR_STREAM( - node_->get_logger(), - get_predicate_details_client_->get_service_name() << - " service client: waiting for service to appear..."); + node_->get_logger(), get_predicate_details_client_->get_service_name() + << " service client: waiting for service to appear..."); } auto request = std::make_shared(); @@ -204,9 +190,9 @@ DomainExpertClient::getPredicate(const std::string & predicate) auto future_result = get_predicate_details_client_->async_send_request(request); - if (rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != - rclcpp::FutureReturnCode::SUCCESS) - { + if ( + rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != + rclcpp::FutureReturnCode::SUCCESS) { return {}; } @@ -216,16 +202,14 @@ DomainExpertClient::getPredicate(const std::string & predicate) return result.node; } else { RCLCPP_ERROR_STREAM( - node_->get_logger(), - get_predicate_details_client_->get_service_name() << ": " << - result.error_info); + node_->get_logger(), get_predicate_details_client_->get_service_name() + << ": " << result.error_info); return {}; } return {}; } -std::vector -DomainExpertClient::getFunctions() +std::vector DomainExpertClient::getFunctions() { std::vector ret; @@ -234,40 +218,36 @@ DomainExpertClient::getFunctions() return ret; } RCLCPP_ERROR_STREAM( - node_->get_logger(), - get_functions_client_->get_service_name() << - " service client: waiting for service to appear..."); + node_->get_logger(), get_functions_client_->get_service_name() + << " service client: waiting for service to appear..."); } auto request = std::make_shared(); auto future_result = get_functions_client_->async_send_request(request); - if (rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != - rclcpp::FutureReturnCode::SUCCESS) - { + if ( + rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != + rclcpp::FutureReturnCode::SUCCESS) { return ret; } auto result = *future_result.get(); - ret = plansys2::convertVector( - result.states); + ret = plansys2::convertVector(result.states); return ret; } -std::optional -DomainExpertClient::getFunction(const std::string & function) +std::optional DomainExpertClient::getFunction(const std::string & function) { while (!get_function_details_client_->wait_for_service(std::chrono::seconds(1))) { if (!rclcpp::ok()) { return {}; } RCLCPP_ERROR_STREAM( - node_->get_logger(), - get_function_details_client_->get_service_name() << - " service client: waiting for service to appear..."); + node_->get_logger(), get_function_details_client_->get_service_name() + << " service client: waiting for service to appear..."); } auto request = std::make_shared(); @@ -276,9 +256,9 @@ DomainExpertClient::getFunction(const std::string & function) auto future_result = get_function_details_client_->async_send_request(request); - if (rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != - rclcpp::FutureReturnCode::SUCCESS) - { + if ( + rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != + rclcpp::FutureReturnCode::SUCCESS) { return {}; } @@ -288,16 +268,14 @@ DomainExpertClient::getFunction(const std::string & function) return result.node; } else { RCLCPP_ERROR_STREAM( - node_->get_logger(), - get_function_details_client_->get_service_name() << ": " << - result.error_info); + node_->get_logger(), get_function_details_client_->get_service_name() + << ": " << result.error_info); return {}; } return {}; } -std::vector -DomainExpertClient::getActions() +std::vector DomainExpertClient::getActions() { std::vector ret; @@ -306,18 +284,17 @@ DomainExpertClient::getActions() return ret; } RCLCPP_ERROR_STREAM( - node_->get_logger(), - get_actions_client_->get_service_name() << - " service client: waiting for service to appear..."); + node_->get_logger(), get_actions_client_->get_service_name() + << " service client: waiting for service to appear..."); } auto request = std::make_shared(); auto future_result = get_actions_client_->async_send_request(request); - if (rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != - rclcpp::FutureReturnCode::SUCCESS) - { + if ( + rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != + rclcpp::FutureReturnCode::SUCCESS) { return ret; } @@ -330,19 +307,16 @@ DomainExpertClient::getActions() return ret; } -plansys2_msgs::msg::Action::SharedPtr -DomainExpertClient::getAction( - const std::string & action, - const std::vector & params) +plansys2_msgs::msg::Action::SharedPtr DomainExpertClient::getAction( + const std::string & action, const std::vector & params) { while (!get_action_details_client_->wait_for_service(std::chrono::seconds(1))) { if (!rclcpp::ok()) { return {}; } RCLCPP_ERROR_STREAM( - node_->get_logger(), - get_action_details_client_->get_service_name() << - " service client: waiting for service to appear..."); + node_->get_logger(), get_action_details_client_->get_service_name() + << " service client: waiting for service to appear..."); } auto request = std::make_shared(); @@ -352,28 +326,25 @@ DomainExpertClient::getAction( auto future_result = get_action_details_client_->async_send_request(request); - if (rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != - rclcpp::FutureReturnCode::SUCCESS) - { + if ( + rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != + rclcpp::FutureReturnCode::SUCCESS) { return {}; } auto result = *future_result.get(); - if (result.success) { return std::make_shared(result.action); } else { RCLCPP_ERROR_STREAM( - node_->get_logger(), - get_action_details_client_->get_service_name() << ": " << - result.error_info); + node_->get_logger(), get_action_details_client_->get_service_name() + << ": " << result.error_info); return {}; } } -std::vector -DomainExpertClient::getDurativeActions() +std::vector DomainExpertClient::getDurativeActions() { std::vector ret; @@ -382,18 +353,17 @@ DomainExpertClient::getDurativeActions() return ret; } RCLCPP_ERROR_STREAM( - node_->get_logger(), - get_durative_actions_client_->get_service_name() << - " service client: waiting for service to appear..."); + node_->get_logger(), get_durative_actions_client_->get_service_name() + << " service client: waiting for service to appear..."); } auto request = std::make_shared(); auto future_result = get_durative_actions_client_->async_send_request(request); - if (rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != - rclcpp::FutureReturnCode::SUCCESS) - { + if ( + rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != + rclcpp::FutureReturnCode::SUCCESS) { return ret; } @@ -406,19 +376,16 @@ DomainExpertClient::getDurativeActions() return ret; } -plansys2_msgs::msg::DurativeAction::SharedPtr -DomainExpertClient::getDurativeAction( - const std::string & action, - const std::vector & params) +plansys2_msgs::msg::DurativeAction::SharedPtr DomainExpertClient::getDurativeAction( + const std::string & action, const std::vector & params) { while (!get_durative_action_details_client_->wait_for_service(std::chrono::seconds(1))) { if (!rclcpp::ok()) { return nullptr; } RCLCPP_ERROR_STREAM( - node_->get_logger(), - get_durative_action_details_client_->get_service_name() << - " service client: waiting for service to appear..."); + node_->get_logger(), get_durative_action_details_client_->get_service_name() + << " service client: waiting for service to appear..."); } auto request = std::make_shared(); @@ -428,28 +395,25 @@ DomainExpertClient::getDurativeAction( auto future_result = get_durative_action_details_client_->async_send_request(request); - if (rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != - rclcpp::FutureReturnCode::SUCCESS) - { + if ( + rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != + rclcpp::FutureReturnCode::SUCCESS) { return nullptr; } auto result = *future_result.get(); if (result.success) { - return std::make_shared( - result.durative_action); + return std::make_shared(result.durative_action); } else { RCLCPP_ERROR_STREAM( - node_->get_logger(), - get_durative_action_details_client_->get_service_name() << ": " << - result.error_info); + node_->get_logger(), get_durative_action_details_client_->get_service_name() + << ": " << result.error_info); return nullptr; } } -std::string -DomainExpertClient::getDomain() +std::string DomainExpertClient::getDomain() { std::string ret; @@ -458,18 +422,17 @@ DomainExpertClient::getDomain() return ret; } RCLCPP_ERROR_STREAM( - node_->get_logger(), - get_domain_client_->get_service_name() << - " service client: waiting for service to appear..."); + node_->get_logger(), get_domain_client_->get_service_name() + << " service client: waiting for service to appear..."); } auto request = std::make_shared(); auto future_result = get_domain_client_->async_send_request(request); - if (rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != - rclcpp::FutureReturnCode::SUCCESS) - { + if ( + rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != + rclcpp::FutureReturnCode::SUCCESS) { return ret; } diff --git a/plansys2_domain_expert/src/plansys2_domain_expert/DomainExpertNode.cpp b/plansys2_domain_expert/src/plansys2_domain_expert/DomainExpertNode.cpp index 1ab3434f..4722c3b4 100644 --- a/plansys2_domain_expert/src/plansys2_domain_expert/DomainExpertNode.cpp +++ b/plansys2_domain_expert/src/plansys2_domain_expert/DomainExpertNode.cpp @@ -14,94 +14,81 @@ #include "plansys2_domain_expert/DomainExpertNode.hpp" -#include #include +#include #include +#include "lifecycle_msgs/msg/state.hpp" #include "plansys2_core/Utils.hpp" #include "plansys2_popf_plan_solver/popf_plan_solver.hpp" -#include "lifecycle_msgs/msg/state.hpp" - namespace plansys2 { - -DomainExpertNode::DomainExpertNode() -: rclcpp_lifecycle::LifecycleNode("domain_expert") +DomainExpertNode::DomainExpertNode() : rclcpp_lifecycle::LifecycleNode("domain_expert") { declare_parameter("model_file", ""); get_name_service_ = create_service( "domain_expert/get_domain_name", std::bind( - &DomainExpertNode::get_domain_name_service_callback, - this, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3)); + &DomainExpertNode::get_domain_name_service_callback, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3)); get_types_service_ = create_service( "domain_expert/get_domain_types", std::bind( - &DomainExpertNode::get_domain_types_service_callback, - this, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3)); + &DomainExpertNode::get_domain_types_service_callback, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3)); get_domain_actions_service_ = create_service( "domain_expert/get_domain_actions", std::bind( - &DomainExpertNode::get_domain_actions_service_callback, - this, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3)); - get_domain_action_details_service_ = - create_service( - "domain_expert/get_domain_action_details", std::bind( - &DomainExpertNode::get_domain_action_details_service_callback, - this, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3)); + &DomainExpertNode::get_domain_actions_service_callback, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3)); + get_domain_action_details_service_ = create_service( + "domain_expert/get_domain_action_details", + std::bind( + &DomainExpertNode::get_domain_action_details_service_callback, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3)); get_domain_durative_actions_service_ = create_service( "domain_expert/get_domain_durative_actions", std::bind( - &DomainExpertNode::get_domain_durative_actions_service_callback, - this, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3)); + &DomainExpertNode::get_domain_durative_actions_service_callback, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3)); get_domain_durative_action_details_service_ = create_service( - "domain_expert/get_domain_durative_action_details", std::bind( - &DomainExpertNode::get_domain_durative_action_details_service_callback, - this, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3)); + "domain_expert/get_domain_durative_action_details", + std::bind( + &DomainExpertNode::get_domain_durative_action_details_service_callback, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); get_domain_predicates_service_ = create_service( - "domain_expert/get_domain_predicates", std::bind( - &DomainExpertNode::get_domain_predicates_service_callback, - this, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3)); - get_domain_predicate_details_service_ = - create_service( - "domain_expert/get_domain_predicate_details", std::bind( - &DomainExpertNode::get_domain_predicate_details_service_callback, - this, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3)); + "domain_expert/get_domain_predicates", + std::bind( + &DomainExpertNode::get_domain_predicates_service_callback, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3)); + get_domain_predicate_details_service_ = create_service( + "domain_expert/get_domain_predicate_details", + std::bind( + &DomainExpertNode::get_domain_predicate_details_service_callback, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3)); get_domain_functions_service_ = create_service( - "domain_expert/get_domain_functions", std::bind( - &DomainExpertNode::get_domain_functions_service_callback, - this, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3)); - get_domain_function_details_service_ = - create_service( - "domain_expert/get_domain_function_details", std::bind( - &DomainExpertNode::get_domain_function_details_service_callback, - this, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3)); + "domain_expert/get_domain_functions", + std::bind( + &DomainExpertNode::get_domain_functions_service_callback, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3)); + get_domain_function_details_service_ = create_service( + "domain_expert/get_domain_function_details", + std::bind( + &DomainExpertNode::get_domain_function_details_service_callback, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3)); get_domain_service_ = create_service( - "domain_expert/get_domain", std::bind( - &DomainExpertNode::get_domain_service_callback, - this, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3)); + "domain_expert/get_domain", + std::bind( + &DomainExpertNode::get_domain_service_callback, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3)); } +using CallbackReturnT = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; -using CallbackReturnT = - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - -CallbackReturnT -DomainExpertNode::on_configure(const rclcpp_lifecycle::State & state) +CallbackReturnT DomainExpertNode::on_configure(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "[%s] Configuring...", get_name()); std::string model_file = get_parameter("model_file").get_value(); @@ -109,9 +96,8 @@ DomainExpertNode::on_configure(const rclcpp_lifecycle::State & state) auto model_files = tokenize(model_file, ":"); std::ifstream domain_ifs(model_files[0]); - std::string domain_str(( - std::istreambuf_iterator(domain_ifs)), - std::istreambuf_iterator()); + std::string domain_str( + (std::istreambuf_iterator(domain_ifs)), std::istreambuf_iterator()); auto planner = std::make_shared(); domain_expert_ = std::make_shared(domain_str); @@ -124,9 +110,8 @@ DomainExpertNode::on_configure(const rclcpp_lifecycle::State & state) for (size_t i = 1; i < model_files.size(); i++) { std::ifstream domain_ifs(model_files[i]); - std::string domain_str(( - std::istreambuf_iterator(domain_ifs)), - std::istreambuf_iterator()); + std::string domain_str( + (std::istreambuf_iterator(domain_ifs)), std::istreambuf_iterator()); domain_expert_->extendDomain(domain_str); bool check_valid = planner->is_valid_domain(domain_expert_->getDomain(), get_namespace()); @@ -141,8 +126,7 @@ DomainExpertNode::on_configure(const rclcpp_lifecycle::State & state) return CallbackReturnT::SUCCESS; } -CallbackReturnT -DomainExpertNode::on_activate(const rclcpp_lifecycle::State & state) +CallbackReturnT DomainExpertNode::on_activate(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "[%s] Activating...", get_name()); RCLCPP_INFO(get_logger(), "[%s] Activated", get_name()); @@ -150,8 +134,7 @@ DomainExpertNode::on_activate(const rclcpp_lifecycle::State & state) return CallbackReturnT::SUCCESS; } -CallbackReturnT -DomainExpertNode::on_deactivate(const rclcpp_lifecycle::State & state) +CallbackReturnT DomainExpertNode::on_deactivate(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "[%s] Deactivating...", get_name()); RCLCPP_INFO(get_logger(), "[%s] Deactivated", get_name()); @@ -159,8 +142,7 @@ DomainExpertNode::on_deactivate(const rclcpp_lifecycle::State & state) return CallbackReturnT::SUCCESS; } -CallbackReturnT -DomainExpertNode::on_cleanup(const rclcpp_lifecycle::State & state) +CallbackReturnT DomainExpertNode::on_cleanup(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "[%s] Cleaning up...", get_name()); RCLCPP_INFO(get_logger(), "[%s] Cleaned up", get_name()); @@ -168,8 +150,7 @@ DomainExpertNode::on_cleanup(const rclcpp_lifecycle::State & state) return CallbackReturnT::SUCCESS; } -CallbackReturnT -DomainExpertNode::on_shutdown(const rclcpp_lifecycle::State & state) +CallbackReturnT DomainExpertNode::on_shutdown(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "[%s] Shutting down...", get_name()); RCLCPP_INFO(get_logger(), "[%s] Shutted down", get_name()); @@ -177,16 +158,14 @@ DomainExpertNode::on_shutdown(const rclcpp_lifecycle::State & state) return CallbackReturnT::SUCCESS; } -CallbackReturnT -DomainExpertNode::on_error(const rclcpp_lifecycle::State & state) +CallbackReturnT DomainExpertNode::on_error(const rclcpp_lifecycle::State & state) { RCLCPP_ERROR(get_logger(), "[%s] Error transition", get_name()); return CallbackReturnT::SUCCESS; } -void -DomainExpertNode::get_domain_name_service_callback( +void DomainExpertNode::get_domain_name_service_callback( const std::shared_ptr request_header, const std::shared_ptr request, const std::shared_ptr response) @@ -201,8 +180,7 @@ DomainExpertNode::get_domain_name_service_callback( } } -void -DomainExpertNode::get_domain_types_service_callback( +void DomainExpertNode::get_domain_types_service_callback( const std::shared_ptr request_header, const std::shared_ptr request, const std::shared_ptr response) @@ -217,8 +195,7 @@ DomainExpertNode::get_domain_types_service_callback( } } -void -DomainExpertNode::get_domain_actions_service_callback( +void DomainExpertNode::get_domain_actions_service_callback( const std::shared_ptr request_header, const std::shared_ptr request, const std::shared_ptr response) @@ -235,8 +212,7 @@ DomainExpertNode::get_domain_actions_service_callback( } } -void -DomainExpertNode::get_domain_action_details_service_callback( +void DomainExpertNode::get_domain_action_details_service_callback( const std::shared_ptr request_header, const std::shared_ptr request, const std::shared_ptr response) @@ -260,8 +236,7 @@ DomainExpertNode::get_domain_action_details_service_callback( } } -void -DomainExpertNode::get_domain_durative_actions_service_callback( +void DomainExpertNode::get_domain_durative_actions_service_callback( const std::shared_ptr request_header, const std::shared_ptr request, const std::shared_ptr response) @@ -278,8 +253,7 @@ DomainExpertNode::get_domain_durative_actions_service_callback( } } -void -DomainExpertNode::get_domain_durative_action_details_service_callback( +void DomainExpertNode::get_domain_durative_action_details_service_callback( const std::shared_ptr request_header, const std::shared_ptr request, const std::shared_ptr response) @@ -305,8 +279,7 @@ DomainExpertNode::get_domain_durative_action_details_service_callback( } } -void -DomainExpertNode::get_domain_predicates_service_callback( +void DomainExpertNode::get_domain_predicates_service_callback( const std::shared_ptr request_header, const std::shared_ptr request, const std::shared_ptr response) @@ -322,8 +295,7 @@ DomainExpertNode::get_domain_predicates_service_callback( } } -void -DomainExpertNode::get_domain_predicate_details_service_callback( +void DomainExpertNode::get_domain_predicate_details_service_callback( const std::shared_ptr request_header, const std::shared_ptr request, const std::shared_ptr response) @@ -339,16 +311,14 @@ DomainExpertNode::get_domain_predicate_details_service_callback( response->success = true; } else { RCLCPP_WARN( - get_logger(), "Requesting a non-existing predicate [%s]", - request->expression.c_str()); + get_logger(), "Requesting a non-existing predicate [%s]", request->expression.c_str()); response->success = false; response->error_info = "Predicate not found"; } } } -void -DomainExpertNode::get_domain_functions_service_callback( +void DomainExpertNode::get_domain_functions_service_callback( const std::shared_ptr request_header, const std::shared_ptr request, const std::shared_ptr response) @@ -364,8 +334,7 @@ DomainExpertNode::get_domain_functions_service_callback( } } -void -DomainExpertNode::get_domain_function_details_service_callback( +void DomainExpertNode::get_domain_function_details_service_callback( const std::shared_ptr request_header, const std::shared_ptr request, const std::shared_ptr response) @@ -381,16 +350,14 @@ DomainExpertNode::get_domain_function_details_service_callback( response->success = true; } else { RCLCPP_WARN( - get_logger(), "Requesting a non-existing function [%s]", - request->expression.c_str()); + get_logger(), "Requesting a non-existing function [%s]", request->expression.c_str()); response->success = false; response->error_info = "Function not found"; } } } -void -DomainExpertNode::get_domain_service_callback( +void DomainExpertNode::get_domain_service_callback( const std::shared_ptr request_header, const std::shared_ptr request, const std::shared_ptr response) @@ -408,5 +375,4 @@ DomainExpertNode::get_domain_service_callback( } } - } // namespace plansys2 diff --git a/plansys2_domain_expert/src/plansys2_domain_expert/DomainReader.cpp b/plansys2_domain_expert/src/plansys2_domain_expert/DomainReader.cpp index acd611b6..460e793c 100644 --- a/plansys2_domain_expert/src/plansys2_domain_expert/DomainReader.cpp +++ b/plansys2_domain_expert/src/plansys2_domain_expert/DomainReader.cpp @@ -14,25 +14,20 @@ #include "plansys2_domain_expert/DomainReader.hpp" -#include -#include -#include #include #include #include +#include +#include +#include #include "plansys2_core/Utils.hpp" - namespace plansys2 { +DomainReader::DomainReader() {} -DomainReader::DomainReader() -{ -} - -void -DomainReader::add_domain(const std::string & domain) +void DomainReader::add_domain(const std::string & domain) { if (domain.empty()) { std::cerr << "Empty domain" << std::endl; @@ -42,9 +37,9 @@ DomainReader::add_domain(const std::string & domain) Domain new_domain; std::string lc_domain = domain; - std::transform( - domain.begin(), domain.end(), lc_domain.begin(), - [](unsigned char c) {return std::tolower(c);}); + std::transform(domain.begin(), domain.end(), lc_domain.begin(), [](unsigned char c) { + return std::tolower(c); + }); lc_domain = remove_comments(lc_domain); @@ -59,8 +54,7 @@ DomainReader::add_domain(const std::string & domain) domains_.push_back(new_domain); } -std::string -DomainReader::get_joint_domain() const +std::string DomainReader::get_joint_domain() const { std::string ret = "(define (domain "; @@ -132,8 +126,7 @@ DomainReader::get_joint_domain() const return ret; } -int -DomainReader::get_end_block(const std::string & domain, std::size_t init_pos) +int DomainReader::get_end_block(const std::string & domain, std::size_t init_pos) { std::size_t domain_length = domain.length(); @@ -158,8 +151,7 @@ DomainReader::get_end_block(const std::string & domain, std::size_t init_pos) } } -std::string -DomainReader::get_name(std::string & domain) +std::string DomainReader::get_name(std::string & domain) { const std::string pattern("domain"); @@ -183,8 +175,7 @@ DomainReader::get_name(std::string & domain) } } -std::string -DomainReader::get_requirements(std::string & domain) +std::string DomainReader::get_requirements(std::string & domain) { const std::string pattern(":requirements"); @@ -207,8 +198,7 @@ DomainReader::get_requirements(std::string & domain) } } -std::string -DomainReader::get_types(const std::string & domain) +std::string DomainReader::get_types(const std::string & domain) { const std::string pattern(":types"); @@ -228,9 +218,7 @@ DomainReader::get_types(const std::string & domain) } } - -std::string -DomainReader::get_constants(const std::string & domain) +std::string DomainReader::get_constants(const std::string & domain) { const std::string pattern(":constants"); @@ -250,9 +238,7 @@ DomainReader::get_constants(const std::string & domain) } } - -std::string -DomainReader::get_predicates(const std::string & domain) +std::string DomainReader::get_predicates(const std::string & domain) { const std::string pattern(":predicates"); @@ -272,8 +258,7 @@ DomainReader::get_predicates(const std::string & domain) } } -std::string -DomainReader::get_functions(const std::string & domain) +std::string DomainReader::get_functions(const std::string & domain) { const std::string pattern(":functions"); @@ -293,8 +278,7 @@ DomainReader::get_functions(const std::string & domain) } } -std::vector -DomainReader::get_actions(const std::string & domain) +std::vector DomainReader::get_actions(const std::string & domain) { std::vector ret; @@ -326,5 +310,4 @@ DomainReader::get_actions(const std::string & domain) return ret; } - } // namespace plansys2 diff --git a/plansys2_domain_expert/test/CMakeLists.txt b/plansys2_domain_expert/test/CMakeLists.txt index 0d63f3ea..0b4105f0 100644 --- a/plansys2_domain_expert/test/CMakeLists.txt +++ b/plansys2_domain_expert/test/CMakeLists.txt @@ -7,4 +7,4 @@ install(DIRECTORY ) add_subdirectory(unit) -#add_subdirectory(integration) \ No newline at end of file +#add_subdirectory(integration) diff --git a/plansys2_domain_expert/test/pddl/domain_charging.pddl b/plansys2_domain_expert/test/pddl/domain_charging.pddl index 5a29ecdb..c68ed811 100644 --- a/plansys2_domain_expert/test/pddl/domain_charging.pddl +++ b/plansys2_domain_expert/test/pddl/domain_charging.pddl @@ -35,7 +35,7 @@ ;; condition - at start -> ?wp1 (waypoint) is connected to ?wp2 (waypoint) ;; at start -> ?r (robot) is at ?wp1 (waypoint) ;; at start -> state_of_charge(?r) >= distance(?wp1, ?wp2) / max_range(?r) - ;; efect - at start -> ?r (robot) is NOT at ?wp1 (waypoint) + ;; effect - at start -> ?r (robot) is NOT at ?wp1 (waypoint) ;; at start -> state_of_charge(?r) -= distance(?wp1, ?wp2) / max_range(?r) ;; at end -> ?r (robot) is at ?wp2 (waypoint) (:durative-action move @@ -55,7 +55,7 @@ ;; Patrol ?wp (waypoint) with ?r (robot). ;; duration = 5 ;; condition - at start -> ?r (robot) is at ?wp (waypoint) - ;; efect - at end -> ?wp (waypoint) has been patrolled + ;; effect - at end -> ?wp (waypoint) has been patrolled (:durative-action patrol :parameters (?r - robot ?wp - waypoint) :duration (= ?duration 5) @@ -67,7 +67,7 @@ ;; duration = 5 ;; condition - at start -> ?r (robot) is at ?wp (waypoint) ;; at start -> ?wp (waypoint) has a charger - ;; efect - at end -> state_of_charge(?r) = 100 + ;; effect - at end -> state_of_charge(?r) = 100 (:durative-action charge :parameters (?r - robot ?wp - waypoint) :duration (= ?duration 5) diff --git a/plansys2_domain_expert/test/pddl/domain_combined_processed.pddl b/plansys2_domain_expert/test/pddl/domain_combined_processed.pddl index 17c18d1b..2886eadb 100644 --- a/plansys2_domain_expert/test/pddl/domain_combined_processed.pddl +++ b/plansys2_domain_expert/test/pddl/domain_combined_processed.pddl @@ -2,7 +2,7 @@ (:requirements :adl :durative-actions :fluents :strips :typing ) (:types -person - object +person - object message - object robot - object room - object @@ -61,7 +61,7 @@ room ) (:action move_person :parameters (?p - person ?r1 ?r2 - room) - :precondition (and + :precondition (and (person_at ?p ?r1) ) :effect (and diff --git a/plansys2_domain_expert/test/pddl/domain_simple.pddl b/plansys2_domain_expert/test/pddl/domain_simple.pddl index 73be9ded..e54cf905 100644 --- a/plansys2_domain_expert/test/pddl/domain_simple.pddl +++ b/plansys2_domain_expert/test/pddl/domain_simple.pddl @@ -3,7 +3,7 @@ ;; Types ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; (:types -person - object +person - object message - object ; This bracket inside a comment, is here for testing purpose :-) robot - object @@ -61,7 +61,7 @@ teleporter_room - room (:action move_person :parameters (?p - person ?r1 ?r2 - room) - :precondition (and + :precondition (and (person_at ?p ?r1) ) :effect (and diff --git a/plansys2_domain_expert/test/pddl/domain_simple_constants.pddl b/plansys2_domain_expert/test/pddl/domain_simple_constants.pddl index 06336e96..e72b656a 100644 --- a/plansys2_domain_expert/test/pddl/domain_simple_constants.pddl +++ b/plansys2_domain_expert/test/pddl/domain_simple_constants.pddl @@ -3,7 +3,7 @@ ;; Types ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; (:types -person - object +person - object message - object ; This bracket inside a comment, is here for testing purpose :-) robot - object @@ -81,7 +81,7 @@ jack john - person (:action move_person_john :parameters (?r1 ?r2 - room) - :precondition (and + :precondition (and (person_at john ?r1) ) :effect (and @@ -93,7 +93,7 @@ jack john - person (:action move_person_jack :parameters (?r1 ?r2 - room) - :precondition (and + :precondition (and (person_at jack ?r1) ) :effect (and diff --git a/plansys2_domain_expert/test/pddl/domain_simple_constants_processed.pddl b/plansys2_domain_expert/test/pddl/domain_simple_constants_processed.pddl index 1ce12d9c..0bcc568b 100644 --- a/plansys2_domain_expert/test/pddl/domain_simple_constants_processed.pddl +++ b/plansys2_domain_expert/test/pddl/domain_simple_constants_processed.pddl @@ -2,7 +2,7 @@ (:requirements :adl :durative-actions :fluents :strips :typing ) (:types -person - object +person - object message - object robot - object room - object @@ -69,7 +69,7 @@ jack john - person ) (:action move_person_john :parameters (?r1 ?r2 - room) - :precondition (and + :precondition (and (person_at john ?r1) ) :effect (and @@ -79,7 +79,7 @@ jack john - person ) (:action move_person_jack :parameters (?r1 ?r2 - room) - :precondition (and + :precondition (and (person_at jack ?r1) ) :effect (and diff --git a/plansys2_domain_expert/test/pddl/domain_simple_processed.pddl b/plansys2_domain_expert/test/pddl/domain_simple_processed.pddl index 137774ee..9eb17b9f 100644 --- a/plansys2_domain_expert/test/pddl/domain_simple_processed.pddl +++ b/plansys2_domain_expert/test/pddl/domain_simple_processed.pddl @@ -2,7 +2,7 @@ (:requirements :adl :durative-actions :fluents :strips :typing ) (:types -person - object +person - object message - object robot - object room - object @@ -56,7 +56,7 @@ teleporter_room - room ) (:action move_person :parameters (?p - person ?r1 ?r2 - room) - :precondition (and + :precondition (and (person_at ?p ?r1) ) :effect (and diff --git a/plansys2_domain_expert/test/pddl/problem_charging.pddl b/plansys2_domain_expert/test/pddl/problem_charging.pddl index d72734e9..44904229 100644 --- a/plansys2_domain_expert/test/pddl/problem_charging.pddl +++ b/plansys2_domain_expert/test/pddl/problem_charging.pddl @@ -2,7 +2,7 @@ (:domain charging) ;; Instantiate the objects. - (:objects + (:objects r2d2 - robot wp_control - waypoint wp1 wp2 wp3 wp4 - waypoint @@ -58,8 +58,8 @@ (patrolled wp3) (patrolled wp4) )) - - (:metric + + (:metric minimize (total-time) ) ) diff --git a/plansys2_domain_expert/test/pddl/problem_simple_1.pddl b/plansys2_domain_expert/test/pddl/problem_simple_1.pddl index bdef79d4..44e5b9ad 100644 --- a/plansys2_domain_expert/test/pddl/problem_simple_1.pddl +++ b/plansys2_domain_expert/test/pddl/problem_simple_1.pddl @@ -15,7 +15,7 @@ ;; The goal is to have both packages delivered to their destinations: (:goal (and - (robot_talk leia m1 Jack) + (robot_talk leia m1 Jack) ) ) ) diff --git a/plansys2_domain_expert/test/unit/CMakeLists.txt b/plansys2_domain_expert/test/unit/CMakeLists.txt index cf4179c2..5a55a776 100644 --- a/plansys2_domain_expert/test/unit/CMakeLists.txt +++ b/plansys2_domain_expert/test/unit/CMakeLists.txt @@ -8,4 +8,4 @@ ament_add_gtest(domain_reader_test domain_reader_test.cpp) target_link_libraries(domain_reader_test ${PROJECT_NAME}) ament_add_gtest(types_test types_test.cpp) -target_link_libraries(types_test ${PROJECT_NAME}) \ No newline at end of file +target_link_libraries(types_test ${PROJECT_NAME}) diff --git a/plansys2_domain_expert/test/unit/domain_expert_node_test.cpp b/plansys2_domain_expert/test/unit/domain_expert_node_test.cpp index beb1dd2a..bd757011 100644 --- a/plansys2_domain_expert/test/unit/domain_expert_node_test.cpp +++ b/plansys2_domain_expert/test/unit/domain_expert_node_test.cpp @@ -12,20 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include #include #include +#include +#include +#include #include "ament_index_cpp/get_package_share_directory.hpp" - #include "gtest/gtest.h" -#include "plansys2_domain_expert/DomainExpertNode.hpp" -#include "plansys2_domain_expert/DomainExpertClient.hpp" - #include "lifecycle_msgs/msg/state.hpp" - +#include "plansys2_domain_expert/DomainExpertClient.hpp" +#include "plansys2_domain_expert/DomainExpertNode.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" @@ -44,8 +41,10 @@ TEST(domain_expert, lifecycle) bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} - }); + while (!finish) { + exe.spin_some(); + } + }); domain_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); @@ -58,8 +57,7 @@ TEST(domain_expert, lifecycle) } ASSERT_EQ( - domain_node->get_current_state().id(), - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + domain_node->get_current_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); domain_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE); @@ -72,8 +70,7 @@ TEST(domain_expert, lifecycle) } ASSERT_EQ( - domain_node->get_current_state().id(), - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + domain_node->get_current_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); auto domain_str = domain_client->getDomain(); @@ -86,9 +83,8 @@ TEST(domain_expert, lifecycle) } std::ifstream domain_ifs_p(pkgpath + "/pddl/domain_simple_processed.pddl"); - std::string domain_str_p(( - std::istreambuf_iterator(domain_ifs_p)), - std::istreambuf_iterator()); + std::string domain_str_p( + (std::istreambuf_iterator(domain_ifs_p)), std::istreambuf_iterator()); ASSERT_EQ(domain_str, domain_str_p); @@ -111,8 +107,10 @@ TEST(domain_expert, lifecycle_error) bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} - }); + while (!finish) { + exe.spin_some(); + } + }); domain_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); @@ -125,14 +123,12 @@ TEST(domain_expert, lifecycle_error) } ASSERT_EQ( - domain_node->get_current_state().id(), - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + domain_node->get_current_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); finish = true; t.join(); } - int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/plansys2_domain_expert/test/unit/domain_expert_test.cpp b/plansys2_domain_expert/test/unit/domain_expert_test.cpp index 2214a59e..60ab86bd 100644 --- a/plansys2_domain_expert/test/unit/domain_expert_test.cpp +++ b/plansys2_domain_expert/test/unit/domain_expert_test.cpp @@ -12,19 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include #include #include +#include +#include +#include #include "ament_index_cpp/get_package_share_directory.hpp" - #include "gtest/gtest.h" #include "plansys2_domain_expert/DomainExpert.hpp" -#include "plansys2_pddl_parser/Utils.h" - #include "plansys2_msgs/msg/node.hpp" +#include "plansys2_pddl_parser/Utils.h" TEST(domain_expert, get_reduced_string) { @@ -52,17 +50,15 @@ TEST(domain_expert, exist_domain) { std::string pkgpath = ament_index_cpp::get_package_share_directory("plansys2_domain_expert"); std::ifstream domain_ifs(pkgpath + "/pddl/domain_charging.pddl"); - std::string domain_str(( - std::istreambuf_iterator(domain_ifs)), - std::istreambuf_iterator()); + std::string domain_str( + (std::istreambuf_iterator(domain_ifs)), std::istreambuf_iterator()); plansys2::DomainExpert domain_expert(domain_str); ASSERT_TRUE(domain_expert.existDomain("charging")); std::ifstream domain_simple_ifs(pkgpath + "/pddl/domain_simple.pddl"); - std::string domain_simple_str(( - std::istreambuf_iterator(domain_simple_ifs)), - std::istreambuf_iterator()); + std::string domain_simple_str( + (std::istreambuf_iterator(domain_simple_ifs)), std::istreambuf_iterator()); domain_expert.extendDomain(domain_simple_str); ASSERT_TRUE(domain_expert.existDomain("plansys2")); @@ -72,14 +68,12 @@ TEST(domain_expert, get_domain) { std::string pkgpath = ament_index_cpp::get_package_share_directory("plansys2_domain_expert"); std::ifstream domain_ifs(pkgpath + "/pddl/domain_simple.pddl"); - std::string domain_str(( - std::istreambuf_iterator(domain_ifs)), - std::istreambuf_iterator()); + std::string domain_str( + (std::istreambuf_iterator(domain_ifs)), std::istreambuf_iterator()); std::ifstream domain_ifs_p(pkgpath + "/pddl/domain_simple_processed.pddl"); - std::string domain_str_p(( - std::istreambuf_iterator(domain_ifs_p)), - std::istreambuf_iterator()); + std::string domain_str_p( + (std::istreambuf_iterator(domain_ifs_p)), std::istreambuf_iterator()); plansys2::DomainExpert domain_expert(domain_str); ASSERT_EQ(domain_expert.getDomain(), domain_str_p); @@ -89,14 +83,12 @@ TEST(domain_expert, get_domain2) { std::string pkgpath = ament_index_cpp::get_package_share_directory("plansys2_domain_expert"); std::ifstream domain_ifs(pkgpath + "/pddl/factory.pddl"); - std::string domain_str(( - std::istreambuf_iterator(domain_ifs)), - std::istreambuf_iterator()); + std::string domain_str( + (std::istreambuf_iterator(domain_ifs)), std::istreambuf_iterator()); std::ifstream domain_ifs_p(pkgpath + "/pddl/factory_processed.pddl"); - std::string domain_str_p(( - std::istreambuf_iterator(domain_ifs_p)), - std::istreambuf_iterator()); + std::string domain_str_p( + (std::istreambuf_iterator(domain_ifs_p)), std::istreambuf_iterator()); plansys2::DomainExpert domain_expert(domain_str); ASSERT_EQ(domain_expert.getDomain(), domain_str_p); @@ -106,9 +98,8 @@ TEST(domain_expert, get_name) { std::string pkgpath = ament_index_cpp::get_package_share_directory("plansys2_domain_expert"); std::ifstream domain_ifs(pkgpath + "/pddl/factory.pddl"); - std::string domain_str(( - std::istreambuf_iterator(domain_ifs)), - std::istreambuf_iterator()); + std::string domain_str( + (std::istreambuf_iterator(domain_ifs)), std::istreambuf_iterator()); plansys2::DomainExpert domain_expert(domain_str); @@ -122,14 +113,12 @@ TEST(domain_expert, get_domain3) { std::string pkgpath = ament_index_cpp::get_package_share_directory("plansys2_domain_expert"); std::ifstream domain_ifs(pkgpath + "/pddl/domain_simple_constants.pddl"); - std::string domain_str(( - std::istreambuf_iterator(domain_ifs)), - std::istreambuf_iterator()); + std::string domain_str( + (std::istreambuf_iterator(domain_ifs)), std::istreambuf_iterator()); std::ifstream domain_ifs_p(pkgpath + "/pddl/domain_simple_constants_processed.pddl"); - std::string domain_str_p(( - std::istreambuf_iterator(domain_ifs_p)), - std::istreambuf_iterator()); + std::string domain_str_p( + (std::istreambuf_iterator(domain_ifs_p)), std::istreambuf_iterator()); plansys2::DomainExpert domain_expert(domain_str); ASSERT_EQ(domain_expert.getDomain(), domain_str_p); @@ -139,14 +128,13 @@ TEST(domain_expert, get_types) { std::string pkgpath = ament_index_cpp::get_package_share_directory("plansys2_domain_expert"); std::ifstream domain_ifs(pkgpath + "/pddl/domain_simple.pddl"); - std::string domain_str(( - std::istreambuf_iterator(domain_ifs)), - std::istreambuf_iterator()); + std::string domain_str( + (std::istreambuf_iterator(domain_ifs)), std::istreambuf_iterator()); plansys2::DomainExpert domain_expert(domain_str); std::vector types = domain_expert.getTypes(); - std::vector test_types {"person", "message", "robot", "room", "teleporter_room"}; + std::vector test_types{"person", "message", "robot", "room", "teleporter_room"}; ASSERT_EQ(types, test_types); } @@ -155,16 +143,15 @@ TEST(domain_expert, get_constants) { std::string pkgpath = ament_index_cpp::get_package_share_directory("plansys2_domain_expert"); std::ifstream domain_ifs(pkgpath + "/pddl/domain_simple_constants.pddl"); - std::string domain_str(( - std::istreambuf_iterator(domain_ifs)), - std::istreambuf_iterator()); + std::string domain_str( + (std::istreambuf_iterator(domain_ifs)), std::istreambuf_iterator()); plansys2::DomainExpert domain_expert(domain_str); std::vector consts1 = domain_expert.getConstants("robot"); std::vector consts2 = domain_expert.getConstants("person"); - std::vector test_consts1 {"leia", "lema"}; - std::vector test_consts2 {"jack", "john"}; + std::vector test_consts1{"leia", "lema"}; + std::vector test_consts2{"jack", "john"}; ASSERT_EQ(consts1, test_consts1); ASSERT_EQ(consts2, test_consts2); @@ -174,15 +161,14 @@ TEST(domain_expert, get_predicates) { std::string pkgpath = ament_index_cpp::get_package_share_directory("plansys2_domain_expert"); std::ifstream domain_ifs(pkgpath + "/pddl/domain_simple.pddl"); - std::string domain_str(( - std::istreambuf_iterator(domain_ifs)), - std::istreambuf_iterator()); + std::string domain_str( + (std::istreambuf_iterator(domain_ifs)), std::istreambuf_iterator()); plansys2::DomainExpert domain_expert(domain_str); std::vector predicates = domain_expert.getPredicates(); - std::vector predicates_types {"person_at", "robot_at", "robot_near_person", - "robot_talk"}; + std::vector predicates_types{ + "person_at", "robot_at", "robot_near_person", "robot_talk"}; ASSERT_EQ(predicates.size(), predicates_types.size()); for (unsigned i = 0; i < predicates.size(); i++) { @@ -194,9 +180,8 @@ TEST(domain_expert, get_predicate_params) { std::string pkgpath = ament_index_cpp::get_package_share_directory("plansys2_domain_expert"); std::ifstream domain_ifs(pkgpath + "/pddl/domain_simple.pddl"); - std::string domain_str(( - std::istreambuf_iterator(domain_ifs)), - std::istreambuf_iterator()); + std::string domain_str( + (std::istreambuf_iterator(domain_ifs)), std::istreambuf_iterator()); plansys2::DomainExpert domain_expert(domain_str); @@ -227,15 +212,13 @@ TEST(domain_expert, get_functions) { std::string pkgpath = ament_index_cpp::get_package_share_directory("plansys2_domain_expert"); std::ifstream domain_ifs(pkgpath + "/pddl/domain_charging.pddl"); - std::string domain_str(( - std::istreambuf_iterator(domain_ifs)), - std::istreambuf_iterator()); + std::string domain_str( + (std::istreambuf_iterator(domain_ifs)), std::istreambuf_iterator()); plansys2::DomainExpert domain_expert(domain_str); std::vector functions = domain_expert.getFunctions(); - std::vector functions_types {"speed", "max_range", "state_of_charge", - "distance"}; + std::vector functions_types{"speed", "max_range", "state_of_charge", "distance"}; ASSERT_EQ(functions.size(), functions_types.size()); for (unsigned i = 0; i < functions.size(); i++) { @@ -247,9 +230,8 @@ TEST(domain_expert, get_function_params) { std::string pkgpath = ament_index_cpp::get_package_share_directory("plansys2_domain_expert"); std::ifstream domain_ifs(pkgpath + "/pddl/domain_charging.pddl"); - std::string domain_str(( - std::istreambuf_iterator(domain_ifs)), - std::istreambuf_iterator()); + std::string domain_str( + (std::istreambuf_iterator(domain_ifs)), std::istreambuf_iterator()); plansys2::DomainExpert domain_expert(domain_str); @@ -276,9 +258,8 @@ TEST(domain_expert, get_actions) { std::string pkgpath = ament_index_cpp::get_package_share_directory("plansys2_domain_expert"); std::ifstream domain_ifs(pkgpath + "/pddl/domain_simple.pddl"); - std::string domain_str(( - std::istreambuf_iterator(domain_ifs)), - std::istreambuf_iterator()); + std::string domain_str( + (std::istreambuf_iterator(domain_ifs)), std::istreambuf_iterator()); plansys2::DomainExpert domain_expert(domain_str); @@ -293,14 +274,12 @@ TEST(domain_expert, get_actions) ASSERT_EQ(durative_actions[2], "approach"); } - TEST(domain_expert, get_action_params) { std::string pkgpath = ament_index_cpp::get_package_share_directory("plansys2_domain_expert"); std::ifstream domain_ifs(pkgpath + "/pddl/domain_simple.pddl"); - std::string domain_str(( - std::istreambuf_iterator(domain_ifs)), - std::istreambuf_iterator()); + std::string domain_str( + (std::istreambuf_iterator(domain_ifs)), std::istreambuf_iterator()); plansys2::DomainExpert domain_expert(domain_str); @@ -326,43 +305,35 @@ TEST(domain_expert, get_action_params) ASSERT_TRUE(parser::pddl::empty(move_action->over_all_requirements)); ASSERT_TRUE(parser::pddl::empty(move_action->at_end_requirements)); - ASSERT_EQ( - parser::pddl::toString(move_action->at_start_requirements), - "(and (robot_at ?0 ?1))"); - ASSERT_EQ( - parser::pddl::toString(move_action->at_start_effects), - "(and (not (robot_at ?0 ?1)))"); - ASSERT_EQ( - parser::pddl::toString(move_action->at_end_effects), - "(and (robot_at ?0 ?2))"); + ASSERT_EQ(parser::pddl::toString(move_action->at_start_requirements), "(and (robot_at ?0 ?1))"); + ASSERT_EQ(parser::pddl::toString(move_action->at_start_effects), "(and (not (robot_at ?0 ?1)))"); + ASSERT_EQ(parser::pddl::toString(move_action->at_end_effects), "(and (robot_at ?0 ?2))"); } TEST(domain_expert, multidomain_get_types) { std::string pkgpath = ament_index_cpp::get_package_share_directory("plansys2_domain_expert"); std::ifstream domain_ifs(pkgpath + "/pddl/domain_simple.pddl"); - std::string domain_str(( - std::istreambuf_iterator(domain_ifs)), - std::istreambuf_iterator()); + std::string domain_str( + (std::istreambuf_iterator(domain_ifs)), std::istreambuf_iterator()); auto domain_expert = std::make_shared(domain_str); std::ifstream domain_ext_ifs(pkgpath + "/pddl/domain_simple_ext.pddl"); - std::string domain_ext_str(( - std::istreambuf_iterator(domain_ext_ifs)), - std::istreambuf_iterator()); + std::string domain_ext_str( + (std::istreambuf_iterator(domain_ext_ifs)), std::istreambuf_iterator()); domain_expert->extendDomain(domain_ext_str); std::vector types = domain_expert->getTypes(); - std::vector test_types {"person", "message", "robot", "room", "teleporter_room", - "pickable_object"}; + std::vector test_types{"person", "message", "robot", + "room", "teleporter_room", "pickable_object"}; ASSERT_EQ(types, test_types); std::vector predicates = domain_expert->getPredicates(); - std::vector test_predicates {"object_at_robot", "object_at_room", "person_at", - "robot_at", "robot_near_person", "robot_talk"}; + std::vector test_predicates{"object_at_robot", "object_at_room", "person_at", + "robot_at", "robot_near_person", "robot_talk"}; ASSERT_EQ(predicates.size(), test_predicates.size()); for (unsigned i = 0; i < predicates.size(); i++) { @@ -370,13 +341,12 @@ TEST(domain_expert, multidomain_get_types) } std::vector actions = domain_expert->getActions(); - std::vector test_actions {"move_person"}; + std::vector test_actions{"move_person"}; ASSERT_EQ(actions, test_actions); std::vector dactions = domain_expert->getDurativeActions(); - std::vector test_dactions {"move", "talk", "approach", "pick_object", - "place_object"}; + std::vector test_dactions{"move", "talk", "approach", "pick_object", "place_object"}; ASSERT_EQ(dactions, test_dactions); } @@ -385,9 +355,8 @@ TEST(domain_expert, sub_types) { std::string pkgpath = ament_index_cpp::get_package_share_directory("plansys2_domain_expert"); std::ifstream domain_ifs(pkgpath + "/pddl/domain_simple.pddl"); - std::string domain_str(( - std::istreambuf_iterator(domain_ifs)), - std::istreambuf_iterator()); + std::string domain_str( + (std::istreambuf_iterator(domain_ifs)), std::istreambuf_iterator()); plansys2::DomainExpert domain_expert(domain_str); @@ -398,7 +367,7 @@ TEST(domain_expert, sub_types) if (durative_action->parameters.size() == 3) { ASSERT_EQ(durative_action->parameters[1].type, "room"); - std::vector subtypes {"teleporter_room"}; + std::vector subtypes{"teleporter_room"}; ASSERT_EQ(durative_action->parameters[1].sub_types, subtypes); } else { FAIL() << "The `move` durative-action is expected to have 2 parameters"; @@ -413,7 +382,7 @@ TEST(domain_expert, sub_types) if (predicate.value().parameters.size() == 2) { ASSERT_EQ(predicate.value().parameters[1].type, "room"); - std::vector subtypes {"teleporter_room"}; + std::vector subtypes{"teleporter_room"}; ASSERT_EQ(predicate.value().parameters[1].sub_types, subtypes); } else { FAIL() << "The `robot_at` predicate is expected to have 3 parameters"; @@ -423,13 +392,12 @@ TEST(domain_expert, sub_types) } // Parameter subtypes with as action - plansys2_msgs::msg::Action::SharedPtr action = - domain_expert.getAction("move_person"); + plansys2_msgs::msg::Action::SharedPtr action = domain_expert.getAction("move_person"); if (action) { if (action->parameters.size() == 3) { ASSERT_EQ(action->parameters[1].type, "room"); - std::vector subtypes {"teleporter_room"}; + std::vector subtypes{"teleporter_room"}; ASSERT_EQ(action->parameters[1].sub_types, subtypes); } else { FAIL() << "The `move_person` action is expected to have 3 parameters"; @@ -439,20 +407,20 @@ TEST(domain_expert, sub_types) } // Parameter subtypes with as function - std::optional function = - domain_expert.getFunction("teleportation_time"); + std::optional function = domain_expert.getFunction("teleportation_time"); if (function.has_value()) { if (function.value().parameters.size() == 2) { ASSERT_EQ(function.value().parameters[0].type, "teleporter_room"); ASSERT_EQ(function.value().parameters[1].type, "room"); - std::vector emptySubtypes {}; + std::vector emptySubtypes{}; ASSERT_EQ(function.value().parameters[0].sub_types, emptySubtypes); - std::vector subtypes {"teleporter_room"}; + std::vector subtypes{"teleporter_room"}; ASSERT_EQ(function.value().parameters[1].sub_types, subtypes); } else { - FAIL() << "The `teleportation_time` function is expected to have 2 parameters"; + FAIL() << "The `teleportation_time` function is expected to have 2 " + "parameters"; } } else { FAIL() << "No `teleportation_time` function found in the domain"; diff --git a/plansys2_domain_expert/test/unit/domain_reader_test.cpp b/plansys2_domain_expert/test/unit/domain_reader_test.cpp index b9de2e63..b08e241a 100644 --- a/plansys2_domain_expert/test/unit/domain_reader_test.cpp +++ b/plansys2_domain_expert/test/unit/domain_reader_test.cpp @@ -12,14 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include +#include #include #include "ament_index_cpp/get_package_share_directory.hpp" - #include "gtest/gtest.h" - #include "plansys2_domain_expert/DomainReader.hpp" class DomainReaderTest : public plansys2::DomainReader @@ -30,49 +28,25 @@ class DomainReaderTest : public plansys2::DomainReader return get_end_block(domain, init_pos); } - std::string get_name_test(std::string & domain) - { - return get_name(domain); - } + std::string get_name_test(std::string & domain) { return get_name(domain); } - std::string get_requirements_test(std::string & domain) - { - return get_requirements(domain); - } + std::string get_requirements_test(std::string & domain) { return get_requirements(domain); } - std::string get_types_test(const std::string & domain) - { - return get_types(domain); - } + std::string get_types_test(const std::string & domain) { return get_types(domain); } - std::string get_constants_test(const std::string & domain) - { - return get_constants(domain); - } + std::string get_constants_test(const std::string & domain) { return get_constants(domain); } - std::string get_predicates_test(const std::string & domain) - { - return get_predicates(domain); - } + std::string get_predicates_test(const std::string & domain) { return get_predicates(domain); } - std::string get_functions_test(const std::string & domain) - { - return get_functions(domain); - } + std::string get_functions_test(const std::string & domain) { return get_functions(domain); } std::vector get_actions_test(const std::string & domain) { return get_actions(domain); } - void add_domain_test(const std::string & domain) - { - add_domain(domain); - } + void add_domain_test(const std::string & domain) { add_domain(domain); } - std::string get_joint_domain_test() - { - return get_joint_domain(); - } + std::string get_joint_domain_test() { return get_joint_domain(); } }; TEST(domain_reader, get_block) @@ -266,7 +240,6 @@ TEST(domain_reader, constants) ASSERT_EQ(res7, req7_estr); } - TEST(domain_reader, predicates) { DomainReaderTest dr; @@ -283,7 +256,6 @@ TEST(domain_reader, predicates) std::string req4_str = ""; std::string req4_estr = ""; - auto res1 = dr.get_predicates_test(req1_str); auto res2 = dr.get_predicates_test(req2_str); auto res3 = dr.get_predicates_test(req3_str); @@ -300,11 +272,13 @@ TEST(domain_reader, functions) DomainReaderTest dr; std::string req1_str = - "(:functions\n(=(robot_at leia bedroom) 10)\n(=(person_at paco kitchen) 30)\n)"; + "(:functions\n(=(robot_at leia bedroom) 10)\n(=(person_at paco kitchen) " + "30)\n)"; std::string req1_estr = "(=(robot_at leia bedroom) 10)\n(=(person_at paco kitchen) 30)"; std::string req2_str = - "(:functions\n(=(robot_at leia bedroom) 10)\n(=(person_at paco kitchen) 30\n)"; + "(:functions\n(=(robot_at leia bedroom) 10)\n(=(person_at paco kitchen) " + "30\n)"; std::string req2_estr = ""; std::string req3_str = "(:functions )"; @@ -313,7 +287,6 @@ TEST(domain_reader, functions) std::string req4_str = ""; std::string req4_estr = ""; - auto res1 = dr.get_functions_test(req1_str); auto res2 = dr.get_functions_test(req2_str); auto res3 = dr.get_functions_test(req3_str); @@ -331,26 +304,23 @@ TEST(domain_reader, actions) std::string req0_str = "(:predicates\n(robot_at leia bedroom) (person_at paco kitchen)\n)"; - std::string req1_str = - "(:action\n whatever \n)"; + std::string req1_str = "(:action\n whatever \n)"; std::string req1_estr = "(:action\n whatever \n)"; - std::string req2_str = - "\n text other (:action\n whatever \n) more text"; + std::string req2_str = "\n text other (:action\n whatever \n) more text"; std::string req2_estr = "(:action\n whatever \n)"; std::string req3_str = std::string("((:types type1 type2)\n(:action\n whatever\n) \n") + - std::string("(:durative-action\n whatever\n)"); + std::string("(:durative-action\n whatever\n)"); std::string req3_1_estr = "(:action\n whatever\n)"; std::string req3_2_estr = "(:durative-action\n whatever\n)"; std::string req4_str = std::string("((:types type1 type2)\n(:action\n whatever\n) \n") + - std::string("(:durative-action\n whatever\n"); + std::string("(:durative-action\n whatever\n"); std::string req4_1_estr = "(:action\n whatever\n)"; std::string req5_str = std::string(""); - auto res0 = dr.get_actions_test(req0_str); auto res1 = dr.get_actions_test(req1_str); auto res2 = dr.get_actions_test(req2_str); @@ -384,14 +354,12 @@ TEST(domain_reader, add_domain) std::string pkgpath = ament_index_cpp::get_package_share_directory("plansys2_domain_expert"); std::ifstream domain_ifs(pkgpath + "/pddl/domain_simple.pddl"); - std::string domain_str(( - std::istreambuf_iterator(domain_ifs)), - std::istreambuf_iterator()); + std::string domain_str( + (std::istreambuf_iterator(domain_ifs)), std::istreambuf_iterator()); std::ifstream domain_ifs_p(pkgpath + "/pddl/domain_simple_processed.pddl"); - std::string domain_str_p(( - std::istreambuf_iterator(domain_ifs_p)), - std::istreambuf_iterator()); + std::string domain_str_p( + (std::istreambuf_iterator(domain_ifs_p)), std::istreambuf_iterator()); dr.add_domain(domain_str); @@ -404,20 +372,16 @@ TEST(domain_reader, add_2_domain) std::string pkgpath = ament_index_cpp::get_package_share_directory("plansys2_domain_expert"); std::ifstream domain_ifs(pkgpath + "/pddl/domain_simple.pddl"); - std::string domain_str(( - std::istreambuf_iterator(domain_ifs)), - std::istreambuf_iterator()); + std::string domain_str( + (std::istreambuf_iterator(domain_ifs)), std::istreambuf_iterator()); std::ifstream domain_ifs_2(pkgpath + "/pddl/domain_simple_ext.pddl"); - std::string domain_str_2(( - std::istreambuf_iterator(domain_ifs_2)), - std::istreambuf_iterator()); - + std::string domain_str_2( + (std::istreambuf_iterator(domain_ifs_2)), std::istreambuf_iterator()); std::ifstream domain_ifs_p(pkgpath + "/pddl/domain_combined_processed.pddl"); - std::string domain_str_p(( - std::istreambuf_iterator(domain_ifs_p)), - std::istreambuf_iterator()); + std::string domain_str_p( + (std::istreambuf_iterator(domain_ifs_p)), std::istreambuf_iterator()); dr.add_domain(domain_str); dr.add_domain(domain_str_2); @@ -431,14 +395,12 @@ TEST(domain_reader, add_domain_with_constants) std::string pkgpath = ament_index_cpp::get_package_share_directory("plansys2_domain_expert"); std::ifstream domain_ifs(pkgpath + "/pddl/domain_simple_constants.pddl"); - std::string domain_str(( - std::istreambuf_iterator(domain_ifs)), - std::istreambuf_iterator()); + std::string domain_str( + (std::istreambuf_iterator(domain_ifs)), std::istreambuf_iterator()); std::ifstream domain_ifs_p(pkgpath + "/pddl/domain_simple_constants_processed.pddl"); - std::string domain_str_p(( - std::istreambuf_iterator(domain_ifs_p)), - std::istreambuf_iterator()); + std::string domain_str_p( + (std::istreambuf_iterator(domain_ifs_p)), std::istreambuf_iterator()); dr.add_domain(domain_str); diff --git a/plansys2_domain_expert/test/unit/types_test.cpp b/plansys2_domain_expert/test/unit/types_test.cpp index 0de0d263..3bc7eac9 100644 --- a/plansys2_domain_expert/test/unit/types_test.cpp +++ b/plansys2_domain_expert/test/unit/types_test.cpp @@ -12,15 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include -#include #include "gtest/gtest.h" -#include "plansys2_pddl_parser/Utils.h" - #include "plansys2_msgs/msg/node.hpp" #include "plansys2_msgs/msg/param.hpp" +#include "plansys2_pddl_parser/Utils.h" TEST(domain_types, basic_types) { @@ -99,8 +98,7 @@ TEST(domain_types, predicate_tree_to_string) auto or_tree = parser::pddl::fromSubtrees(or_subtrees, plansys2_msgs::msg::Node::OR); ASSERT_EQ( - parser::pddl::toString( - *or_tree), "(or (person_at paco bedroom)(person_at paco kitchen))"); + parser::pddl::toString(*or_tree), "(or (person_at paco bedroom)(person_at paco kitchen))"); std::vector and_subtrees; and_subtrees.push_back(pn_1); @@ -110,7 +108,8 @@ TEST(domain_types, predicate_tree_to_string) ASSERT_EQ( parser::pddl::toString(*and_tree), std::string("(and (robot_at r2d2 bedroom)(not ") + - std::string("(robot_at r2d2 kitchen))(or (person_at paco bedroom)(person_at paco kitchen)))")); + std::string("(robot_at r2d2 kitchen))(or (person_at paco " + "bedroom)(person_at paco kitchen)))")); } TEST(domain_types, predicate_tree_to_string_2) @@ -185,8 +184,7 @@ TEST(domain_types, predicate_tree_to_string_2) auto or_tree = parser::pddl::fromSubtrees(or_subtrees, plansys2_msgs::msg::Node::OR); ASSERT_EQ( - parser::pddl::toString( - *or_tree), "(or (person_at paco bedroom)(person_at paco kitchen))"); + parser::pddl::toString(*or_tree), "(or (person_at paco bedroom)(person_at paco kitchen))"); std::vector and_subtrees; and_subtrees.push_back(pn_1); @@ -196,7 +194,8 @@ TEST(domain_types, predicate_tree_to_string_2) ASSERT_EQ( parser::pddl::toString(*and_tree), std::string("(and (robot_at r2d2 bedroom)") + - std::string("(robot_at r2d2 kitchen)(or (person_at paco bedroom)(person_at paco kitchen)))")); + std::string("(robot_at r2d2 kitchen)(or (person_at paco " + "bedroom)(person_at paco kitchen)))")); } TEST(domain_types, predicate_tree_to_string_3) @@ -271,8 +270,7 @@ TEST(domain_types, predicate_tree_to_string_3) auto or_tree = parser::pddl::fromSubtrees(or_subtrees, plansys2_msgs::msg::Node::OR); ASSERT_EQ( - parser::pddl::toString( - *or_tree), "(or (person_at paco bedroom)(person_at paco kitchen))"); + parser::pddl::toString(*or_tree), "(or (person_at paco bedroom)(person_at paco kitchen))"); std::vector not_subtrees_3; not_subtrees_3.push_back(*or_tree); @@ -289,18 +287,21 @@ TEST(domain_types, predicate_tree_to_string_3) auto and_tree = parser::pddl::fromSubtrees(and_subtrees, plansys2_msgs::msg::Node::AND); ASSERT_EQ( - parser::pddl::toString(*and_tree), std::string("(and (robot_at r2d2 bedroom)") + - std::string("(robot_at r2d2 kitchen)(and (not (person_at paco ") + - std::string("bedroom))(not (person_at paco kitchen))))")); + parser::pddl::toString(*and_tree), + std::string("(and (robot_at r2d2 bedroom)") + + std::string("(robot_at r2d2 kitchen)(and (not (person_at paco ") + + std::string("bedroom))(not (person_at paco kitchen))))")); } TEST(domain_types, predicate_tree_from_string) { - std::string expresion = std::string("(and (robot_at r2d2 bedroom)(not ") + - std::string("(robot_at r2d2 kitchen))(or (person_at paco bedroom)(person_at paco kitchen)))"); + std::string expression = std::string("(and (robot_at r2d2 bedroom)(not ") + + std::string( + "(robot_at r2d2 kitchen))(or (person_at paco " + "bedroom)(person_at paco kitchen)))"); plansys2_msgs::msg::Tree tree; - parser::pddl::fromString(tree, expresion); + parser::pddl::fromString(tree, expression); plansys2_msgs::msg::Node and_node = tree.nodes[0]; ASSERT_EQ(and_node.node_type, plansys2_msgs::msg::Node::AND); @@ -324,7 +325,7 @@ TEST(domain_types, predicate_tree_from_string) ASSERT_EQ(p2_node.parameters[1].name, "kitchen"); ASSERT_TRUE(p2_node.negate); - ASSERT_EQ(parser::pddl::toString(tree), expresion); + ASSERT_EQ(parser::pddl::toString(tree), expression); std::string expresion2 = std::string("(and (person_at ?0 ?2)(not (person_at ?0 ?1)))"); plansys2_msgs::msg::Tree tree2; @@ -335,11 +336,11 @@ TEST(domain_types, predicate_tree_from_string) TEST(domain_types, predicate_tree_from_string_2) { - std::string expresion = std::string("(not (and (robot_at r2d2 bedroom)") + - std::string("(robot_at r2d2 kitchen)))"); + std::string expression = + std::string("(not (and (robot_at r2d2 bedroom)") + std::string("(robot_at r2d2 kitchen)))"); plansys2_msgs::msg::Tree tree; - parser::pddl::fromString(tree, expresion); + parser::pddl::fromString(tree, expression); plansys2_msgs::msg::Node not_node = tree.nodes[0]; ASSERT_EQ(not_node.node_type, plansys2_msgs::msg::Node::NOT); @@ -364,7 +365,7 @@ TEST(domain_types, predicate_tree_from_string_2) ASSERT_TRUE(p2_node.negate); std::string expresion_eq = std::string("(or (not (robot_at r2d2 bedroom))") + - std::string("(not (robot_at r2d2 kitchen)))"); + std::string("(not (robot_at r2d2 kitchen)))"); ASSERT_EQ(parser::pddl::toString(tree), expresion_eq); @@ -377,31 +378,33 @@ TEST(domain_types, predicate_tree_from_string_2) TEST(domain_types, predicate_tree_from_string_3) { - std::string expresion = std::string("(and (patrolled ro1) (patrolled ro2) (patrolled ro3))"); + std::string expression = std::string("(and (patrolled ro1) (patrolled ro2) (patrolled ro3))"); plansys2_msgs::msg::Tree tree; - parser::pddl::fromString(tree, expresion); + parser::pddl::fromString(tree, expression); ASSERT_EQ(parser::pddl::toString(tree), "(and (patrolled ro1)(patrolled ro2)(patrolled ro3))"); } TEST(domain_types, predicate_tree_from_string_4) { - std::string expresion = std::string(" ( and (patrolled ro1) (patrolled ro2) (patrolled ro3))"); + std::string expression = std::string(" ( and (patrolled ro1) (patrolled ro2) (patrolled ro3))"); plansys2_msgs::msg::Tree tree; - parser::pddl::fromString(tree, expresion); + parser::pddl::fromString(tree, expression); ASSERT_EQ(parser::pddl::toString(tree), "(and (patrolled ro1)(patrolled ro2)(patrolled ro3))"); } TEST(domain_types, predicate_tree_from_string_negative) { - std::string expresion = std::string("(and (robot_at r2d2 bedroom)(not ") + - std::string("(robot_at r2d2 kitchen))(or (person_at paco bedroom)(person_at paco kitchen)))"); + std::string expression = std::string("(and (robot_at r2d2 bedroom)(not ") + + std::string( + "(robot_at r2d2 kitchen))(or (person_at paco " + "bedroom)(person_at paco kitchen)))"); plansys2_msgs::msg::Tree tree; - parser::pddl::fromString(tree, expresion); + parser::pddl::fromString(tree, expression); - ASSERT_EQ(parser::pddl::toString(tree), expresion); + ASSERT_EQ(parser::pddl::toString(tree), expression); std::string expresion2 = std::string("(and (person_at ?0 ?2)(not (person_at ?0 ?1)))"); plansys2_msgs::msg::Tree tree2; @@ -413,7 +416,9 @@ TEST(domain_types, predicate_tree_from_string_negative) TEST(domain_types, get_predicates) { std::string expresion_1 = std::string("(and (robot_at r2d2 bedroom)(not ") + - std::string("(robot_at r2d2 kitchen))(or (person_at paco bedroom)(person_at paco kitchen)))"); + std::string( + "(robot_at r2d2 kitchen))(or (person_at paco " + "bedroom)(person_at paco kitchen)))"); plansys2_msgs::msg::Tree tree_1; parser::pddl::fromString(tree_1, expresion_1); diff --git a/plansys2_executor/CMakeLists.txt b/plansys2_executor/CMakeLists.txt index 7faa2ed6..ce29d445 100644 --- a/plansys2_executor/CMakeLists.txt +++ b/plansys2_executor/CMakeLists.txt @@ -113,6 +113,7 @@ install(TARGETS if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) + set(ament_cmake_uncrustify_FOUND TRUE) ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest REQUIRED) diff --git a/plansys2_executor/README.md b/plansys2_executor/README.md index e260968a..c22ff014 100644 --- a/plansys2_executor/README.md +++ b/plansys2_executor/README.md @@ -2,9 +2,9 @@ The Executor module is responsible for requesting a plan to the Planner, and carry it out, calling to the nodes in the client application that implements the actions. While executing each action, it checks the requisites (*At Start, At End and Over all*, in case of durative Actions). If the requirements are not meet, it cancels the plan execution. It also is responsible for applying the effects of the actions, requesting updates to the Problem Expert. -The main class of Executor is [`plansys2::ExecutorNode`](include/include/plansys2_executor/ExcutorNode.hpp), which is instantiated from [`executor_node.cpp`](src/executor_node.cpp). +The main class of Executor is [`plansys2::ExecutorNode`](include/include/plansys2_executor/ExcutorNode.hpp), which is instantiated from [`executor_node.cpp`](src/executor_node.cpp). -The executions of plans are carried out using ROS2 actions, in particular, [`plansys2_msgs::action::ExecutePlan`](../plansys2_msgs/action/ExecutePlan.action). Take note that the goal must be already in the Domain Expert. +The executions of plans are carried out using ROS2 actions, in particular, [`plansys2_msgs::action::ExecutePlan`](../plansys2_msgs/action/ExecutePlan.action). Take note that the goal must be already in the Domain Expert. ExecutorNode ask for the domain and problem, and ask for a plan to the Planner. For each action in the plan, ExecuterNode creates a [`plansys2::ActionExecutor`](include/include/plansys2_executor/ActionExecutor.hpp). The lifetime of this object is only one action. This object calls the actions implemented in the client appliciation using the ROS2 actions [`plansys2_msgs::action::ExecuteAction`](../plansys2_msgs/action/ExecuteAction.action). Each client action implementation can use the class [`plansys2::ActionExecutorClient`](include/include/plansys2_executor/ActionExecutorClient.hpp) to avoid the complexity of managing ROS2 actions. diff --git a/plansys2_executor/behavior_trees/plansys2_action_bt.xml b/plansys2_executor/behavior_trees/plansys2_action_bt.xml index b87f4831..c7245f17 100644 --- a/plansys2_executor/behavior_trees/plansys2_action_bt.xml +++ b/plansys2_executor/behavior_trees/plansys2_action_bt.xml @@ -8,4 +8,4 @@ WAIT_PREV_ACTIONS - \ No newline at end of file + diff --git a/plansys2_executor/include/plansys2_executor/ActionExecutor.hpp b/plansys2_executor/include/plansys2_executor/ActionExecutor.hpp index bca2a8de..1b10e78a 100644 --- a/plansys2_executor/include/plansys2_executor/ActionExecutor.hpp +++ b/plansys2_executor/include/plansys2_executor/ActionExecutor.hpp @@ -15,38 +15,27 @@ #ifndef PLANSYS2_EXECUTOR__ACTIONEXECUTOR_HPP_ #define PLANSYS2_EXECUTOR__ACTIONEXECUTOR_HPP_ -#include #include +#include #include +#include "behaviortree_cpp_v3/behavior_tree.h" #include "plansys2_msgs/msg/action_execution.hpp" #include "plansys2_msgs/msg/action_execution_info.hpp" #include "plansys2_msgs/msg/durative_action.hpp" -#include "behaviortree_cpp_v3/behavior_tree.h" - #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" namespace plansys2 { - class ActionExecutor { public: - enum Status - { - IDLE, - DEALING, - RUNNING, - SUCCESS, - FAILURE, - CANCELLED - }; + enum Status { IDLE, DEALING, RUNNING, SUCCESS, FAILURE, CANCELLED }; using Ptr = std::shared_ptr; static Ptr make_shared( - const std::string & action, - rclcpp_lifecycle::LifecycleNode::SharedPtr node) + const std::string & action, rclcpp_lifecycle::LifecycleNode::SharedPtr node) { return std::make_shared(action, node); } @@ -60,17 +49,17 @@ class ActionExecutor bool is_finished(); // Methods for debug - Status get_internal_status() const {return state_;} - void set_internal_status(Status state) {state_ = state;} - std::string get_action_name() const {return action_name_;} - std::vector get_action_params() const {return action_params_;} + Status get_internal_status() const { return state_; } + void set_internal_status(Status state) { state_ = state; } + std::string get_action_name() const { return action_name_; } + std::vector get_action_params() const { return action_params_; } plansys2_msgs::msg::ActionExecution last_msg; - rclcpp::Time get_start_time() const {return start_execution_;} - rclcpp::Time get_status_time() const {return state_time_;} + rclcpp::Time get_start_time() const { return start_execution_; } + rclcpp::Time get_status_time() const { return state_time_; } - std::string get_feedback() const {return feedback_;} - float get_completion() const {return completion_;} + std::string get_feedback() const { return feedback_; } + float get_completion() const { return completion_; } protected: rclcpp_lifecycle::LifecycleNode::SharedPtr node_; diff --git a/plansys2_executor/include/plansys2_executor/ActionExecutorClient.hpp b/plansys2_executor/include/plansys2_executor/ActionExecutorClient.hpp index 8aa7f3d3..2953404a 100644 --- a/plansys2_executor/include/plansys2_executor/ActionExecutorClient.hpp +++ b/plansys2_executor/include/plansys2_executor/ActionExecutorClient.hpp @@ -15,23 +15,20 @@ #ifndef PLANSYS2_EXECUTOR__ACTIONEXECUTORCLIENT_HPP_ #define PLANSYS2_EXECUTOR__ACTIONEXECUTORCLIENT_HPP_ -#include #include +#include #include +#include "plansys2_domain_expert/DomainExpertClient.hpp" #include "plansys2_msgs/msg/action_execution.hpp" #include "plansys2_msgs/msg/action_performer_status.hpp" - -#include "plansys2_domain_expert/DomainExpertClient.hpp" #include "plansys2_problem_expert/ProblemExpertClient.hpp" - #include "rclcpp/rclcpp.hpp" -#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp" #include "rclcpp_action/rclcpp_action.hpp" +#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp" namespace plansys2 { - class ActionExecutorClient : public rclcpp_cascade_lifecycle::CascadeLifecycleNode { public: @@ -41,20 +38,17 @@ class ActionExecutorClient : public rclcpp_cascade_lifecycle::CascadeLifecycleNo return std::make_shared(node_name, rate); } - ActionExecutorClient( - const std::string & node_name, - const std::chrono::nanoseconds & rate); + ActionExecutorClient(const std::string & node_name, const std::chrono::nanoseconds & rate); - plansys2_msgs::msg::ActionPerformerStatus get_internal_status() const {return status_;} + plansys2_msgs::msg::ActionPerformerStatus get_internal_status() const { return status_; } protected: virtual void do_work() {} - const std::vector & get_arguments() const {return current_arguments_;} - const std::string get_action_name() const {return action_managed_;} + const std::vector & get_arguments() const { return current_arguments_; } + const std::string get_action_name() const { return action_managed_; } - using CallbackReturnT = - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + using CallbackReturnT = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; virtual CallbackReturnT on_configure(const rclcpp_lifecycle::State & state); virtual CallbackReturnT on_activate(const rclcpp_lifecycle::State & state); diff --git a/plansys2_executor/include/plansys2_executor/BTBuilder.hpp b/plansys2_executor/include/plansys2_executor/BTBuilder.hpp index 0090db38..a5b55c1b 100644 --- a/plansys2_executor/include/plansys2_executor/BTBuilder.hpp +++ b/plansys2_executor/include/plansys2_executor/BTBuilder.hpp @@ -24,17 +24,7 @@ namespace plansys2 { - -enum struct ActionType -{ - UNKNOWN, - INIT, - DURATIVE, - START, - OVERALL, - END, - GOAL -}; +enum struct ActionType { UNKNOWN, INIT, DURATIVE, START, OVERALL, END, GOAL }; struct ActionStamped { @@ -44,8 +34,7 @@ struct ActionStamped ActionType type; std::shared_ptr action; - ActionStamped() - : time(0.0), duration(0.0) {} + ActionStamped() : time(0.0), duration(0.0) {} }; class BTBuilder @@ -54,15 +43,13 @@ class BTBuilder using Ptr = std::shared_ptr; virtual void initialize( - const std::string & bt_action_1 = "", - const std::string & bt_action_2 = "", + const std::string & bt_action_1 = "", const std::string & bt_action_2 = "", int precision = 3) = 0; virtual std::string get_tree(const plansys2_msgs::msg::Plan & current_plan) = 0; virtual std::string get_dotgraph( std::shared_ptr> action_map, - bool enable_legend = false, - bool enable_print_graph = false) = 0; + bool enable_legend = false, bool enable_print_graph = false) = 0; static int to_int_time(float time, int power) { diff --git a/plansys2_executor/include/plansys2_executor/ComputeBT.hpp b/plansys2_executor/include/plansys2_executor/ComputeBT.hpp index 617e30cc..d0d5ff62 100644 --- a/plansys2_executor/include/plansys2_executor/ComputeBT.hpp +++ b/plansys2_executor/include/plansys2_executor/ComputeBT.hpp @@ -41,12 +41,10 @@ namespace plansys2 { - class ComputeBT : public rclcpp_lifecycle::LifecycleNode { public: - using CallbackReturnT = - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + using CallbackReturnT = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; ComputeBT(); diff --git a/plansys2_executor/include/plansys2_executor/ExecutorClient.hpp b/plansys2_executor/include/plansys2_executor/ExecutorClient.hpp index 288d5694..1eb1bab0 100644 --- a/plansys2_executor/include/plansys2_executor/ExecutorClient.hpp +++ b/plansys2_executor/include/plansys2_executor/ExecutorClient.hpp @@ -15,23 +15,21 @@ #ifndef PLANSYS2_EXECUTOR__EXECUTORCLIENT_HPP_ #define PLANSYS2_EXECUTOR__EXECUTORCLIENT_HPP_ +#include #include #include -#include #include #include "plansys2_msgs/action/execute_plan.hpp" -#include "plansys2_msgs/srv/get_ordered_sub_goals.hpp" -#include "plansys2_msgs/srv/get_plan.hpp" #include "plansys2_msgs/msg/plan.hpp" #include "plansys2_msgs/msg/tree.hpp" - +#include "plansys2_msgs/srv/get_ordered_sub_goals.hpp" +#include "plansys2_msgs/srv/get_plan.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" namespace plansys2 { - class ExecutorClient { public: @@ -47,15 +45,14 @@ class ExecutorClient std::vector getOrderedSubGoals(); std::optional getPlan(); - ExecutePlan::Feedback getFeedBack() {return feedback_;} + ExecutePlan::Feedback getFeedBack() { return feedback_; } std::optional getResult(); private: rclcpp::Node::SharedPtr node_; rclcpp_action::Client::SharedPtr action_client_; - rclcpp::Client::SharedPtr - get_ordered_sub_goals_client_; + rclcpp::Client::SharedPtr get_ordered_sub_goals_client_; rclcpp::Client::SharedPtr get_plan_client_; ExecutePlan::Feedback feedback_; diff --git a/plansys2_executor/include/plansys2_executor/ExecutorNode.hpp b/plansys2_executor/include/plansys2_executor/ExecutorNode.hpp index bfbc9e21..6214e515 100644 --- a/plansys2_executor/include/plansys2_executor/ExecutorNode.hpp +++ b/plansys2_executor/include/plansys2_executor/ExecutorNode.hpp @@ -15,44 +15,38 @@ #ifndef PLANSYS2_EXECUTOR__EXECUTORNODE_HPP_ #define PLANSYS2_EXECUTOR__EXECUTORNODE_HPP_ +#include #include -#include #include -#include +#include +#include "lifecycle_msgs/msg/state.hpp" +#include "lifecycle_msgs/msg/transition.hpp" #include "plansys2_domain_expert/DomainExpertClient.hpp" -#include "plansys2_problem_expert/ProblemExpertClient.hpp" -#include "plansys2_planner/PlannerClient.hpp" #include "plansys2_executor/ActionExecutor.hpp" #include "plansys2_executor/BTBuilder.hpp" - -#include "lifecycle_msgs/msg/state.hpp" -#include "lifecycle_msgs/msg/transition.hpp" - #include "plansys2_msgs/action/execute_plan.hpp" #include "plansys2_msgs/msg/action_execution_info.hpp" -#include "plansys2_msgs/srv/get_ordered_sub_goals.hpp" #include "plansys2_msgs/msg/plan.hpp" -#include "std_msgs/msg/string.hpp" - +#include "plansys2_msgs/srv/get_ordered_sub_goals.hpp" +#include "plansys2_planner/PlannerClient.hpp" +#include "plansys2_problem_expert/ProblemExpertClient.hpp" +#include "pluginlib/class_list_macros.hpp" +#include "pluginlib/class_loader.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "rclcpp_lifecycle/lifecycle_publisher.hpp" - -#include "pluginlib/class_loader.hpp" -#include "pluginlib/class_list_macros.hpp" +#include "std_msgs/msg/string.hpp" namespace plansys2 { - class ExecutorNode : public rclcpp_lifecycle::LifecycleNode { public: using ExecutePlan = plansys2_msgs::action::ExecutePlan; using GoalHandleExecutePlan = rclcpp_action::ServerGoalHandle; - using CallbackReturnT = - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + using CallbackReturnT = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; ExecutorNode(); @@ -94,8 +88,7 @@ class ExecutorNode : public rclcpp_lifecycle::LifecycleNode rclcpp_lifecycle::LifecyclePublisher::SharedPtr executing_plan_pub_; rclcpp_action::Server::SharedPtr execute_plan_action_server_; - rclcpp::Service::SharedPtr - get_ordered_sub_goals_service_; + rclcpp::Service::SharedPtr get_ordered_sub_goals_service_; rclcpp_lifecycle::LifecyclePublisher::SharedPtr dotgraph_pub_; std::optional> getOrderedSubGoals(); @@ -103,8 +96,7 @@ class ExecutorNode : public rclcpp_lifecycle::LifecycleNode rclcpp::Service::SharedPtr get_plan_service_; rclcpp_action::GoalResponse handle_goal( - const rclcpp_action::GoalUUID & uuid, - std::shared_ptr goal); + const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal); rclcpp_action::CancelResponse handle_cancel( const std::shared_ptr goal_handle); @@ -116,8 +108,7 @@ class ExecutorNode : public rclcpp_lifecycle::LifecycleNode std::vector get_feedback_info( std::shared_ptr> action_map); - void print_execution_info( - std::shared_ptr> exec_info); + void print_execution_info(std::shared_ptr> exec_info); }; } // namespace plansys2 diff --git a/plansys2_executor/include/plansys2_executor/behavior_tree/apply_atend_effect_node.hpp b/plansys2_executor/include/plansys2_executor/behavior_tree/apply_atend_effect_node.hpp index 12fdf34d..3de6963c 100644 --- a/plansys2_executor/include/plansys2_executor/behavior_tree/apply_atend_effect_node.hpp +++ b/plansys2_executor/include/plansys2_executor/behavior_tree/apply_atend_effect_node.hpp @@ -16,36 +16,30 @@ #define PLANSYS2_EXECUTOR__BEHAVIOR_TREE__APPLY_ATEND_EFFECT_NODE_HPP_ #include -#include #include +#include #include "behaviortree_cpp_v3/action_node.h" - -#include "plansys2_problem_expert/ProblemExpertClient.hpp" #include "plansys2_executor/ActionExecutor.hpp" -#include "plansys2_problem_expert/Utils.hpp" - #include "plansys2_executor/behavior_tree/execute_action_node.hpp" +#include "plansys2_problem_expert/ProblemExpertClient.hpp" +#include "plansys2_problem_expert/Utils.hpp" namespace plansys2 { - class ApplyAtEndEffect : public BT::ActionNodeBase { public: - ApplyAtEndEffect( - const std::string & xml_tag_name, - const BT::NodeConfiguration & conf); + ApplyAtEndEffect(const std::string & xml_tag_name, const BT::NodeConfiguration & conf); void halt() {} BT::NodeStatus tick() override; static BT::PortsList providedPorts() { - return BT::PortsList( - { - BT::InputPort("action", "Action whose at end reqs must stop"), - }); + return BT::PortsList({ + BT::InputPort("action", "Action whose at end reqs must stop"), + }); } private: diff --git a/plansys2_executor/include/plansys2_executor/behavior_tree/apply_atstart_effect_node.hpp b/plansys2_executor/include/plansys2_executor/behavior_tree/apply_atstart_effect_node.hpp index 200a6660..6eaf8bf6 100644 --- a/plansys2_executor/include/plansys2_executor/behavior_tree/apply_atstart_effect_node.hpp +++ b/plansys2_executor/include/plansys2_executor/behavior_tree/apply_atstart_effect_node.hpp @@ -16,36 +16,30 @@ #define PLANSYS2_EXECUTOR__BEHAVIOR_TREE__APPLY_ATSTART_EFFECT_NODE_HPP_ #include -#include #include +#include #include "behaviortree_cpp_v3/action_node.h" - -#include "plansys2_problem_expert/ProblemExpertClient.hpp" #include "plansys2_executor/ActionExecutor.hpp" -#include "plansys2_problem_expert/Utils.hpp" - #include "plansys2_executor/behavior_tree/execute_action_node.hpp" +#include "plansys2_problem_expert/ProblemExpertClient.hpp" +#include "plansys2_problem_expert/Utils.hpp" namespace plansys2 { - class ApplyAtStartEffect : public BT::ActionNodeBase { public: - ApplyAtStartEffect( - const std::string & xml_tag_name, - const BT::NodeConfiguration & conf); + ApplyAtStartEffect(const std::string & xml_tag_name, const BT::NodeConfiguration & conf); void halt() {} BT::NodeStatus tick() override; static BT::PortsList providedPorts() { - return BT::PortsList( - { - BT::InputPort("action", "Action whose at end reqs must stop"), - }); + return BT::PortsList({ + BT::InputPort("action", "Action whose at end reqs must stop"), + }); } private: diff --git a/plansys2_executor/include/plansys2_executor/behavior_tree/check_action_node.hpp b/plansys2_executor/include/plansys2_executor/behavior_tree/check_action_node.hpp index e28b2983..e00ae836 100644 --- a/plansys2_executor/include/plansys2_executor/behavior_tree/check_action_node.hpp +++ b/plansys2_executor/include/plansys2_executor/behavior_tree/check_action_node.hpp @@ -16,35 +16,28 @@ #define PLANSYS2_EXECUTOR__BEHAVIOR_TREE__CHECK_ACTION_NODE_HPP_ #include -#include #include - +#include #include "behaviortree_cpp_v3/action_node.h" - #include "plansys2_executor/ActionExecutor.hpp" - #include "plansys2_executor/behavior_tree/execute_action_node.hpp" namespace plansys2 { - class CheckAction : public BT::ActionNodeBase { public: - CheckAction( - const std::string & xml_tag_name, - const BT::NodeConfiguration & conf); + CheckAction(const std::string & xml_tag_name, const BT::NodeConfiguration & conf); void halt() {} BT::NodeStatus tick() override; static BT::PortsList providedPorts() { - return BT::PortsList( - { - BT::InputPort("action", "Action to be executed"), - }); + return BT::PortsList({ + BT::InputPort("action", "Action to be executed"), + }); } private: diff --git a/plansys2_executor/include/plansys2_executor/behavior_tree/check_atend_req_node.hpp b/plansys2_executor/include/plansys2_executor/behavior_tree/check_atend_req_node.hpp index fb8a2540..62b6146d 100644 --- a/plansys2_executor/include/plansys2_executor/behavior_tree/check_atend_req_node.hpp +++ b/plansys2_executor/include/plansys2_executor/behavior_tree/check_atend_req_node.hpp @@ -16,36 +16,30 @@ #define PLANSYS2_EXECUTOR__BEHAVIOR_TREE__CHECK_ATEND_REQ_NODE_HPP_ #include -#include #include +#include #include "behaviortree_cpp_v3/action_node.h" - -#include "plansys2_problem_expert/ProblemExpertClient.hpp" #include "plansys2_executor/ActionExecutor.hpp" -#include "plansys2_problem_expert/Utils.hpp" - #include "plansys2_executor/behavior_tree/execute_action_node.hpp" +#include "plansys2_problem_expert/ProblemExpertClient.hpp" +#include "plansys2_problem_expert/Utils.hpp" namespace plansys2 { - class CheckAtEndReq : public BT::ActionNodeBase { public: - CheckAtEndReq( - const std::string & xml_tag_name, - const BT::NodeConfiguration & conf); + CheckAtEndReq(const std::string & xml_tag_name, const BT::NodeConfiguration & conf); void halt() {} BT::NodeStatus tick() override; static BT::PortsList providedPorts() { - return BT::PortsList( - { - BT::InputPort("action", "Action whose at end reqs must stop"), - }); + return BT::PortsList({ + BT::InputPort("action", "Action whose at end reqs must stop"), + }); } private: diff --git a/plansys2_executor/include/plansys2_executor/behavior_tree/check_overall_req_node.hpp b/plansys2_executor/include/plansys2_executor/behavior_tree/check_overall_req_node.hpp index bcb1cfdf..b851d539 100644 --- a/plansys2_executor/include/plansys2_executor/behavior_tree/check_overall_req_node.hpp +++ b/plansys2_executor/include/plansys2_executor/behavior_tree/check_overall_req_node.hpp @@ -16,36 +16,30 @@ #define PLANSYS2_EXECUTOR__BEHAVIOR_TREE__CHECK_OVERALL_REQ_NODE_HPP_ #include -#include #include +#include #include "behaviortree_cpp_v3/action_node.h" - -#include "plansys2_problem_expert/ProblemExpertClient.hpp" #include "plansys2_executor/ActionExecutor.hpp" -#include "plansys2_problem_expert/Utils.hpp" - #include "plansys2_executor/behavior_tree/execute_action_node.hpp" +#include "plansys2_problem_expert/ProblemExpertClient.hpp" +#include "plansys2_problem_expert/Utils.hpp" namespace plansys2 { - class CheckOverAllReq : public BT::ActionNodeBase { public: - CheckOverAllReq( - const std::string & xml_tag_name, - const BT::NodeConfiguration & conf); + CheckOverAllReq(const std::string & xml_tag_name, const BT::NodeConfiguration & conf); void halt() {} BT::NodeStatus tick() override; static BT::PortsList providedPorts() { - return BT::PortsList( - { - BT::InputPort("action", "Action whose over all reqs must stop"), - }); + return BT::PortsList({ + BT::InputPort("action", "Action whose over all reqs must stop"), + }); } private: diff --git a/plansys2_executor/include/plansys2_executor/behavior_tree/check_timeout_node.hpp b/plansys2_executor/include/plansys2_executor/behavior_tree/check_timeout_node.hpp index 3c51857e..c8acf5d1 100644 --- a/plansys2_executor/include/plansys2_executor/behavior_tree/check_timeout_node.hpp +++ b/plansys2_executor/include/plansys2_executor/behavior_tree/check_timeout_node.hpp @@ -16,36 +16,30 @@ #define PLANSYS2_EXECUTOR__BEHAVIOR_TREE__CHECK_TIMEOUT_NODE_HPP_ #include -#include #include +#include #include "behaviortree_cpp_v3/action_node.h" - -#include "plansys2_problem_expert/ProblemExpertClient.hpp" #include "plansys2_executor/ActionExecutor.hpp" -#include "plansys2_problem_expert/Utils.hpp" - #include "plansys2_executor/behavior_tree/execute_action_node.hpp" +#include "plansys2_problem_expert/ProblemExpertClient.hpp" +#include "plansys2_problem_expert/Utils.hpp" namespace plansys2 { - class CheckTimeout : public BT::ActionNodeBase { public: - CheckTimeout( - const std::string & xml_tag_name, - const BT::NodeConfiguration & conf); + CheckTimeout(const std::string & xml_tag_name, const BT::NodeConfiguration & conf); void halt() {} BT::NodeStatus tick() override; static BT::PortsList providedPorts() { - return BT::PortsList( - { - BT::InputPort("action", "Action whose over all reqs must stop"), - }); + return BT::PortsList({ + BT::InputPort("action", "Action whose over all reqs must stop"), + }); } private: diff --git a/plansys2_executor/include/plansys2_executor/behavior_tree/execute_action_node.hpp b/plansys2_executor/include/plansys2_executor/behavior_tree/execute_action_node.hpp index 6c51f3dc..9e5e1dc2 100644 --- a/plansys2_executor/include/plansys2_executor/behavior_tree/execute_action_node.hpp +++ b/plansys2_executor/include/plansys2_executor/behavior_tree/execute_action_node.hpp @@ -16,36 +16,30 @@ #define PLANSYS2_EXECUTOR__BEHAVIOR_TREE__EXECUTE_ACTION_NODE_HPP_ #include -#include #include #include +#include #include "behaviortree_cpp_v3/action_node.h" - #include "plansys2_executor/ActionExecutor.hpp" - #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" namespace plansys2 { - class ExecuteAction : public BT::ActionNodeBase { public: - ExecuteAction( - const std::string & xml_tag_name, - const BT::NodeConfiguration & conf); + ExecuteAction(const std::string & xml_tag_name, const BT::NodeConfiguration & conf); void halt(); BT::NodeStatus tick() override; static BT::PortsList providedPorts() { - return BT::PortsList( - { - BT::InputPort("action", "Action to be executed"), - }); + return BT::PortsList({ + BT::InputPort("action", "Action to be executed"), + }); } private: diff --git a/plansys2_executor/include/plansys2_executor/behavior_tree/wait_action_node.hpp b/plansys2_executor/include/plansys2_executor/behavior_tree/wait_action_node.hpp index 1c07cc9b..590cf1ec 100644 --- a/plansys2_executor/include/plansys2_executor/behavior_tree/wait_action_node.hpp +++ b/plansys2_executor/include/plansys2_executor/behavior_tree/wait_action_node.hpp @@ -16,35 +16,28 @@ #define PLANSYS2_EXECUTOR__BEHAVIOR_TREE__WAIT_ACTION_NODE_HPP_ #include -#include #include - +#include #include "behaviortree_cpp_v3/action_node.h" - #include "plansys2_executor/ActionExecutor.hpp" - #include "plansys2_executor/behavior_tree/execute_action_node.hpp" namespace plansys2 { - class WaitAction : public BT::ActionNodeBase { public: - WaitAction( - const std::string & xml_tag_name, - const BT::NodeConfiguration & conf); + WaitAction(const std::string & xml_tag_name, const BT::NodeConfiguration & conf); void halt() {} BT::NodeStatus tick() override; static BT::PortsList providedPorts() { - return BT::PortsList( - { - BT::InputPort("action", "Action to be executed"), - }); + return BT::PortsList({ + BT::InputPort("action", "Action to be executed"), + }); } private: diff --git a/plansys2_executor/include/plansys2_executor/behavior_tree/wait_atstart_req_node.hpp b/plansys2_executor/include/plansys2_executor/behavior_tree/wait_atstart_req_node.hpp index 5308e306..d6e6dc0a 100644 --- a/plansys2_executor/include/plansys2_executor/behavior_tree/wait_atstart_req_node.hpp +++ b/plansys2_executor/include/plansys2_executor/behavior_tree/wait_atstart_req_node.hpp @@ -16,36 +16,30 @@ #define PLANSYS2_EXECUTOR__BEHAVIOR_TREE__WAIT_ATSTART_REQ_NODE_HPP_ #include -#include #include +#include #include "behaviortree_cpp_v3/action_node.h" - -#include "plansys2_problem_expert/ProblemExpertClient.hpp" #include "plansys2_executor/ActionExecutor.hpp" -#include "plansys2_problem_expert/Utils.hpp" - #include "plansys2_executor/behavior_tree/execute_action_node.hpp" +#include "plansys2_problem_expert/ProblemExpertClient.hpp" +#include "plansys2_problem_expert/Utils.hpp" namespace plansys2 { - class WaitAtStartReq : public BT::ActionNodeBase { public: - WaitAtStartReq( - const std::string & xml_tag_name, - const BT::NodeConfiguration & conf); + WaitAtStartReq(const std::string & xml_tag_name, const BT::NodeConfiguration & conf); void halt() {} BT::NodeStatus tick() override; static BT::PortsList providedPorts() { - return BT::PortsList( - { - BT::InputPort("action", "Action whose at start reqs must stop"), - }); + return BT::PortsList({ + BT::InputPort("action", "Action whose at start reqs must stop"), + }); } private: diff --git a/plansys2_executor/include/plansys2_executor/bt_builder_plugins/simple_bt_builder.hpp b/plansys2_executor/include/plansys2_executor/bt_builder_plugins/simple_bt_builder.hpp index 4bdf8894..bf5a167a 100644 --- a/plansys2_executor/include/plansys2_executor/bt_builder_plugins/simple_bt_builder.hpp +++ b/plansys2_executor/include/plansys2_executor/bt_builder_plugins/simple_bt_builder.hpp @@ -15,35 +15,32 @@ #ifndef PLANSYS2_EXECUTOR__BT_BUILDER_PLUGINS__SIMPLE_BT_BUILDER_HPP_ #define PLANSYS2_EXECUTOR__BT_BUILDER_PLUGINS__SIMPLE_BT_BUILDER_HPP_ -#include -#include -#include -#include #include #include -#include +#include +#include +#include #include +#include +#include -#include "std_msgs/msg/empty.hpp" - +#include "plansys2_core/Types.hpp" #include "plansys2_domain_expert/DomainExpertClient.hpp" -#include "plansys2_problem_expert/ProblemExpertClient.hpp" #include "plansys2_executor/ActionExecutor.hpp" #include "plansys2_executor/BTBuilder.hpp" -#include "plansys2_core/Types.hpp" #include "plansys2_msgs/msg/durative_action.hpp" #include "plansys2_msgs/msg/plan.hpp" - +#include "plansys2_problem_expert/ProblemExpertClient.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" +#include "std_msgs/msg/empty.hpp" namespace plansys2 { - struct GraphNode { using Ptr = std::shared_ptr; - static Ptr make_shared() {return std::make_shared();} + static Ptr make_shared() { return std::make_shared(); } ActionStamped action; int node_num; @@ -59,7 +56,7 @@ struct GraphNode struct Graph { using Ptr = std::shared_ptr; - static Ptr make_shared() {return std::make_shared();} + static Ptr make_shared() { return std::make_shared(); } std::list roots; std::map> levels; @@ -70,15 +67,12 @@ class SimpleBTBuilder : public BTBuilder public: SimpleBTBuilder(); void initialize( - const std::string & bt_action_1 = "", - const std::string & bt_action_2 = "", - int precision = 3); + const std::string & bt_action_1 = "", const std::string & bt_action_2 = "", int precision = 3); std::string get_tree(const plansys2_msgs::msg::Plan & current_plan); std::string get_dotgraph( std::shared_ptr> action_map, - bool enable_legend = false, - bool enable_print_graph = false); + bool enable_legend = false, bool enable_print_graph = false); protected: std::shared_ptr domain_client_; @@ -94,67 +88,52 @@ class SimpleBTBuilder : public BTBuilder void prune_backwards(GraphNode::Ptr new_node, GraphNode::Ptr node_satisfy); void prune_forward(GraphNode::Ptr current, std::list & used_nodes); void get_state( - const GraphNode::Ptr & node, - std::list & used_nodes, + const GraphNode::Ptr & node, std::list & used_nodes, std::vector & predicates, std::vector & functions) const; bool is_action_executable( - const ActionStamped & action, - std::vector & predicates, + const ActionStamped & action, std::vector & predicates, std::vector & functions) const; std::list get_roots( std::vector & action_sequence, - std::vector & predicates, - std::vector & functions, + std::vector & predicates, std::vector & functions, int & node_counter); GraphNode::Ptr get_node_satisfy( - const plansys2_msgs::msg::Tree & requirement, - const Graph::Ptr & graph, + const plansys2_msgs::msg::Tree & requirement, const Graph::Ptr & graph, const GraphNode::Ptr & current); GraphNode::Ptr get_node_satisfy( - const plansys2_msgs::msg::Tree & requirement, - const GraphNode::Ptr & node, + const plansys2_msgs::msg::Tree & requirement, const GraphNode::Ptr & node, const GraphNode::Ptr & current); std::list get_node_contradict( - const Graph::Ptr & graph, - const GraphNode::Ptr & current); + const Graph::Ptr & graph, const GraphNode::Ptr & current); void get_node_contradict( - const GraphNode::Ptr & node, - const GraphNode::Ptr & current, + const GraphNode::Ptr & node, const GraphNode::Ptr & current, std::list & parents); void remove_existing_requirements( std::vector & requirements, std::vector & predicates, std::vector & functions) const; bool is_parallelizable( - const plansys2::ActionStamped & action, - const std::vector & predicates, - const std::vector & functions, - const std::list & ret) const; + const plansys2::ActionStamped & action, const std::vector & predicates, + const std::vector & functions, const std::list & ret) const; std::string get_flow_tree( - GraphNode::Ptr node, - std::list & used_nodes, - int level = 0); + GraphNode::Ptr node, std::list & used_nodes, int level = 0); void get_flow_dotgraph(GraphNode::Ptr node, std::set & edges); std::string get_node_dotgraph( - GraphNode::Ptr node, std::shared_ptr> action_map, int level = 0); + GraphNode::Ptr node, std::shared_ptr> action_map, + int level = 0); ActionExecutor::Status get_action_status( - ActionStamped action, - std::shared_ptr> action_map); + ActionStamped action, std::shared_ptr> action_map); void addDotGraphLegend( - std::stringstream & ss, int tab_level, int level_counter, - int node_counter); + std::stringstream & ss, int tab_level, int level_counter, int node_counter); std::string t(int level); std::string execution_block(const GraphNode::Ptr & node, int l); void print_node( - const GraphNode::Ptr & node, - int level, - std::set & used_nodes) const; + const GraphNode::Ptr & node, int level, std::set & used_nodes) const; void print_graph(const plansys2::Graph::Ptr & graph) const; @@ -162,8 +141,7 @@ class SimpleBTBuilder : public BTBuilder void print_graph_csv(const plansys2::Graph::Ptr & graph) const; void get_node_tabular( - const plansys2::GraphNode::Ptr & node, - uint32_t root_num, + const plansys2::GraphNode::Ptr & node, uint32_t root_num, std::vector> & graph) const; std::vector> get_graph_tabular( const plansys2::Graph::Ptr & graph) const; @@ -171,7 +149,6 @@ class SimpleBTBuilder : public BTBuilder } // namespace plansys2 - #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS(plansys2::SimpleBTBuilder, plansys2::BTBuilder) diff --git a/plansys2_executor/include/plansys2_executor/bt_builder_plugins/stn_bt_builder.hpp b/plansys2_executor/include/plansys2_executor/bt_builder_plugins/stn_bt_builder.hpp index 3afedc8b..9220e865 100644 --- a/plansys2_executor/include/plansys2_executor/bt_builder_plugins/stn_bt_builder.hpp +++ b/plansys2_executor/include/plansys2_executor/bt_builder_plugins/stn_bt_builder.hpp @@ -32,7 +32,6 @@ namespace plansys2 { - struct StateVec { std::vector predicates; @@ -42,7 +41,7 @@ struct StateVec struct GraphNode { using Ptr = std::shared_ptr; - static Ptr make_shared(int id) {return std::make_shared(id);} + static Ptr make_shared(int id) { return std::make_shared(id); } int node_num; ActionStamped action; @@ -50,14 +49,13 @@ struct GraphNode std::set> input_arcs; std::set> output_arcs; - explicit GraphNode(int id) - : node_num(id) {} + explicit GraphNode(int id) : node_num(id) {} }; struct Graph { using Ptr = std::shared_ptr; - static Ptr make_shared() {return std::make_shared();} + static Ptr make_shared() { return std::make_shared(); } std::list nodes; }; @@ -67,15 +65,12 @@ class STNBTBuilder : public BTBuilder public: STNBTBuilder(); void initialize( - const std::string & bt_action_1 = "", - const std::string & bt_action_2 = "", - int precision = 3); + const std::string & bt_action_1 = "", const std::string & bt_action_2 = "", int precision = 3); std::string get_tree(const plansys2_msgs::msg::Plan & current_plan); std::string get_dotgraph( std::shared_ptr> action_map, - bool enable_legend = false, - bool enable_print_graph = false); + bool enable_legend = false, bool enable_print_graph = false); protected: Graph::Ptr build_stn(const plansys2_msgs::msg::Plan & plan) const; @@ -91,43 +86,30 @@ class STNBTBuilder : public BTBuilder std::multimap get_simple_plan(const plansys2_msgs::msg::Plan & plan) const; std::map get_states( - const std::set & happenings, - const std::multimap & plan) const; + const std::set & happenings, const std::multimap & plan) const; plansys2_msgs::msg::Tree from_state( const std::vector & preds, const std::vector & funcs) const; - std::vector get_nodes( - const ActionStamped & action, - const Graph::Ptr graph) const; + std::vector get_nodes(const ActionStamped & action, const Graph::Ptr graph) const; - bool is_match( - const GraphNode::Ptr node, - const ActionStamped & action) const; + bool is_match(const GraphNode::Ptr node, const ActionStamped & action) const; std::vector> get_parents( - const std::pair & action, - const std::multimap & plan, - const std::set & happenings, - const std::map & states) const; + const std::pair & action, const std::multimap & plan, + const std::set & happenings, const std::map & states) const; std::vector> get_satisfy( - const std::pair & action, - const std::multimap & plan, - const std::set & happenings, - const std::map & states) const; + const std::pair & action, const std::multimap & plan, + const std::set & happenings, const std::map & states) const; std::vector> get_threat( - const std::pair & action, - const std::multimap & plan, - const std::set & happenings, - const std::map & states) const; + const std::pair & action, const std::multimap & plan, + const std::set & happenings, const std::map & states) const; bool can_apply( - const std::pair & action, - const std::multimap & plan, - const int & time, - StateVec & state) const; + const std::pair & action, const std::multimap & plan, + const int & time, StateVec & state) const; StateVec get_diff(const StateVec & X_1, const StateVec & X_2) const; StateVec get_intersection(const StateVec & X_1, const StateVec & X_2) const; @@ -139,40 +121,27 @@ class STNBTBuilder : public BTBuilder bool check_paths(GraphNode::Ptr current, GraphNode::Ptr previous) const; std::string get_flow( - const GraphNode::Ptr node, - const GraphNode::Ptr parent, - std::set & used, + const GraphNode::Ptr node, const GraphNode::Ptr parent, std::set & used, const int & level) const; std::string start_execution_block( - const GraphNode::Ptr node, - const GraphNode::Ptr parent, - const int & l) const; + const GraphNode::Ptr node, const GraphNode::Ptr parent, const int & l) const; std::string end_execution_block( - const GraphNode::Ptr node, - const GraphNode::Ptr parent, - const int & l) const; + const GraphNode::Ptr node, const GraphNode::Ptr parent, const int & l) const; - void get_flow_dotgraph( - GraphNode::Ptr node, - std::set & edges); + void get_flow_dotgraph(GraphNode::Ptr node, std::set & edges); std::string get_node_dotgraph( - GraphNode::Ptr node, - std::shared_ptr> action_map); + GraphNode::Ptr node, std::shared_ptr> action_map); ActionExecutor::Status get_action_status( - ActionStamped action, - std::shared_ptr> action_map); - std::string add_dot_graph_legend( - int level_counter, - int node_counter); + ActionStamped action, std::shared_ptr> action_map); + std::string add_dot_graph_legend(int level_counter, int node_counter); void print_graph(const plansys2::Graph::Ptr graph) const; void print_node(const GraphNode::Ptr node, int level) const; void replace(std::string & str, const std::string & from, const std::string & to) const; bool is_end( - const std::tuple & edge, - const ActionStamped & action) const; + const std::tuple & edge, const ActionStamped & action) const; std::string t(const int & level) const; @@ -187,7 +156,6 @@ class STNBTBuilder : public BTBuilder } // namespace plansys2 - #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS(plansys2::STNBTBuilder, plansys2::BTBuilder) diff --git a/plansys2_executor/launch/compute_bt_launch.py b/plansys2_executor/launch/compute_bt_launch.py index cd62c9bf..7b4c6858 100644 --- a/plansys2_executor/launch/compute_bt_launch.py +++ b/plansys2_executor/launch/compute_bt_launch.py @@ -35,18 +35,16 @@ def generate_launch_description(): use_groot = LaunchConfiguration("use_groot") use_rqt_dotgraph = LaunchConfiguration("use_rqt_dotgraph") - namespace = LaunchConfiguration('namespace') - action_bt_file = LaunchConfiguration('action_bt_file') - start_action_bt_file = LaunchConfiguration('start_action_bt_file') - end_action_bt_file = LaunchConfiguration('end_action_bt_file') + namespace = LaunchConfiguration("namespace") + action_bt_file = LaunchConfiguration("action_bt_file") + start_action_bt_file = LaunchConfiguration("start_action_bt_file") + end_action_bt_file = LaunchConfiguration("end_action_bt_file") bt_builder_plugin = LaunchConfiguration("bt_builder_plugin") domain = LaunchConfiguration("domain") problem = LaunchConfiguration("problem") declare_use_groot_cmd = DeclareLaunchArgument( - "use_groot", - default_value="False", - description="Whether to start groot" + "use_groot", default_value="False", description="Whether to start groot" ) declare_use_rqt_dotgraph_cmd = DeclareLaunchArgument( @@ -56,30 +54,38 @@ def generate_launch_description(): ) declare_namespace_cmd = DeclareLaunchArgument( - 'namespace', - default_value='ai_planning', - description='Namespace') + "namespace", default_value="ai_planning", description="Namespace" + ) declare_action_bt_file_cmd = DeclareLaunchArgument( - 'action_bt_file', + "action_bt_file", default_value=os.path.join( - get_package_share_directory('plansys2_executor'), - 'behavior_trees', 'plansys2_action_bt.xml'), - description='BT representing a PDDL action') + get_package_share_directory("plansys2_executor"), + "behavior_trees", + "plansys2_action_bt.xml", + ), + description="BT representing a PDDL action", + ) declare_start_action_bt_file_cmd = DeclareLaunchArgument( - 'start_action_bt_file', + "start_action_bt_file", default_value=os.path.join( - get_package_share_directory('plansys2_executor'), - 'behavior_trees', 'plansys2_start_action_bt.xml'), - description='BT representing a PDDL start action') + get_package_share_directory("plansys2_executor"), + "behavior_trees", + "plansys2_start_action_bt.xml", + ), + description="BT representing a PDDL start action", + ) declare_end_action_bt_file_cmd = DeclareLaunchArgument( - 'end_action_bt_file', + "end_action_bt_file", default_value=os.path.join( - get_package_share_directory('plansys2_executor'), - 'behavior_trees', 'plansys2_end_action_bt.xml'), - description='BT representing a PDDL end action') + get_package_share_directory("plansys2_executor"), + "behavior_trees", + "plansys2_end_action_bt.xml", + ), + description="BT representing a PDDL end action", + ) declare_bt_builder_plugin_cmd = DeclareLaunchArgument( "bt_builder_plugin", @@ -90,16 +96,20 @@ def generate_launch_description(): declare_domain_cmd = DeclareLaunchArgument( "domain", default_value=os.path.join( - get_package_share_directory('plansys2_executor'), - 'pddl', 'road_trip_domain.pddl'), + get_package_share_directory("plansys2_executor"), + "pddl", + "road_trip_domain.pddl", + ), description="PDDL domain file.", ) declare_problem_cmd = DeclareLaunchArgument( "problem", default_value=os.path.join( - get_package_share_directory('plansys2_executor'), - 'pddl', 'road_trip_problem.pddl'), + get_package_share_directory("plansys2_executor"), + "pddl", + "road_trip_problem.pddl", + ), description="PDDL domain file.", ) @@ -130,19 +140,20 @@ def generate_launch_description(): ) compute_bt_cmd = Node( - package='plansys2_executor', - executable='compute_bt', - name='compute_bt', + package="plansys2_executor", + executable="compute_bt", + name="compute_bt", namespace=namespace, - output='screen', + output="screen", parameters=[ - {'action_bt_xml_filename': action_bt_file}, - {'start_action_bt_xml_filename': start_action_bt_file}, - {'end_action_bt_xml_filename': end_action_bt_file}, - {'bt_builder_plugin': bt_builder_plugin}, - {'domain': domain}, - {'problem': problem}, - ]) + {"action_bt_xml_filename": action_bt_file}, + {"start_action_bt_xml_filename": start_action_bt_file}, + {"end_action_bt_xml_filename": end_action_bt_file}, + {"bt_builder_plugin": bt_builder_plugin}, + {"domain": domain}, + {"problem": problem}, + ], + ) visualization_group_cmd = GroupAction( [ diff --git a/plansys2_executor/launch/executor_launch.py b/plansys2_executor/launch/executor_launch.py index a17fa671..bac2cc9a 100644 --- a/plansys2_executor/launch/executor_launch.py +++ b/plansys2_executor/launch/executor_launch.py @@ -1,3 +1,4 @@ +# -*- coding: utf-8 -*- # Copyright 2019 Intelligent Robotics Lab # # Licensed under the Apache License, Version 2.0 (the "License"); @@ -23,38 +24,46 @@ def generate_launch_description(): - namespace = LaunchConfiguration('namespace') - params_file = LaunchConfiguration('params_file') - action_bt_file = LaunchConfiguration('action_bt_file') - start_action_bt_file = LaunchConfiguration('start_action_bt_file') - end_action_bt_file = LaunchConfiguration('end_action_bt_file') + namespace = LaunchConfiguration("namespace") + params_file = LaunchConfiguration("params_file") + action_bt_file = LaunchConfiguration("action_bt_file") + start_action_bt_file = LaunchConfiguration("start_action_bt_file") + end_action_bt_file = LaunchConfiguration("end_action_bt_file") bt_builder_plugin = LaunchConfiguration("bt_builder_plugin") declare_namespace_cmd = DeclareLaunchArgument( - 'namespace', - default_value='', - description='Namespace') + "namespace", default_value="", description="Namespace" + ) declare_action_bt_file_cmd = DeclareLaunchArgument( - 'action_bt_file', + "action_bt_file", default_value=os.path.join( - get_package_share_directory('plansys2_executor'), - 'behavior_trees', 'plansys2_action_bt.xml'), - description='BT representing a PDDL action') + get_package_share_directory("plansys2_executor"), + "behavior_trees", + "plansys2_action_bt.xml", + ), + description="BT representing a PDDL action", + ) declare_start_action_bt_file_cmd = DeclareLaunchArgument( - 'start_action_bt_file', + "start_action_bt_file", default_value=os.path.join( - get_package_share_directory('plansys2_executor'), - 'behavior_trees', 'plansys2_start_action_bt.xml'), - description='BT representing a PDDL start action') + get_package_share_directory("plansys2_executor"), + "behavior_trees", + "plansys2_start_action_bt.xml", + ), + description="BT representing a PDDL start action", + ) declare_end_action_bt_file_cmd = DeclareLaunchArgument( - 'end_action_bt_file', + "end_action_bt_file", default_value=os.path.join( - get_package_share_directory('plansys2_executor'), - 'behavior_trees', 'plansys2_end_action_bt.xml'), - description='BT representing a PDDL end action') + get_package_share_directory("plansys2_executor"), + "behavior_trees", + "plansys2_end_action_bt.xml", + ), + description="BT representing a PDDL end action", + ) declare_bt_builder_plugin_cmd = DeclareLaunchArgument( "bt_builder_plugin", @@ -64,18 +73,19 @@ def generate_launch_description(): # Specify the actions executor_cmd = Node( - package='plansys2_executor', - executable='executor_node', - name='executor', + package="plansys2_executor", + executable="executor_node", + name="executor", namespace=namespace, - output='screen', + output="screen", parameters=[ - {'default_action_bt_xml_filename': action_bt_file}, - {'default_start_action_bt_xml_filename': start_action_bt_file}, - {'default_end_action_bt_xml_filename': end_action_bt_file}, - {'bt_builder_plugin': bt_builder_plugin}, - params_file - ]) + {"default_action_bt_xml_filename": action_bt_file}, + {"default_start_action_bt_xml_filename": start_action_bt_file}, + {"default_end_action_bt_xml_filename": end_action_bt_file}, + {"bt_builder_plugin": bt_builder_plugin}, + params_file, + ], + ) # Create the launch description and populate ld = LaunchDescription() diff --git a/plansys2_executor/package.xml b/plansys2_executor/package.xml index 8af5b126..9e8d8358 100644 --- a/plansys2_executor/package.xml +++ b/plansys2_executor/package.xml @@ -3,11 +3,11 @@ plansys2_executor 2.0.8 - + This package contains the Executor module for the ROS2 Planning System Francisco Martin Rico - + Apache License, Version 2.0 ament_cmake diff --git a/plansys2_executor/src/plansys2_executor/ActionExecutor.cpp b/plansys2_executor/src/plansys2_executor/ActionExecutor.cpp index d5dd449f..013ac4b2 100644 --- a/plansys2_executor/src/plansys2_executor/ActionExecutor.cpp +++ b/plansys2_executor/src/plansys2_executor/ActionExecutor.cpp @@ -12,23 +12,21 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "plansys2_executor/ActionExecutor.hpp" + #include +#include #include #include "plansys2_pddl_parser/Utils.h" -#include "plansys2_executor/ActionExecutor.hpp" - namespace plansys2 { - using std::placeholders::_1; using namespace std::chrono_literals; ActionExecutor::ActionExecutor( - const std::string & action, - rclcpp_lifecycle::LifecycleNode::SharedPtr node) + const std::string & action, rclcpp_lifecycle::LifecycleNode::SharedPtr node) : node_(node), state_(IDLE), completion_(0.0) { action_hub_pub_ = node_->create_publisher( @@ -46,8 +44,7 @@ ActionExecutor::ActionExecutor( state_time_ = start_execution_; } -void -ActionExecutor::action_hub_callback(const plansys2_msgs::msg::ActionExecution::SharedPtr msg) +void ActionExecutor::action_hub_callback(const plansys2_msgs::msg::ActionExecution::SharedPtr msg) { last_msg = *msg; @@ -73,9 +70,9 @@ ActionExecutor::action_hub_callback(const plansys2_msgs::msg::ActionExecution::S } break; case plansys2_msgs::msg::ActionExecution::FEEDBACK: - if (state_ != RUNNING || msg->arguments != action_params_ || msg->action != action_name_ || - msg->node_id != current_performer_id_) - { + if ( + state_ != RUNNING || msg->arguments != action_params_ || msg->action != action_name_ || + msg->node_id != current_performer_id_) { return; } feedback_ = msg->status; @@ -84,9 +81,9 @@ ActionExecutor::action_hub_callback(const plansys2_msgs::msg::ActionExecution::S break; case plansys2_msgs::msg::ActionExecution::FINISH: - if (msg->arguments == action_params_ && - msg->action == action_name_ && msg->node_id == current_performer_id_) - { + if ( + msg->arguments == action_params_ && msg->action == action_name_ && + msg->node_id == current_performer_id_) { if (msg->success) { state_ = SUCCESS; } else { @@ -105,14 +102,13 @@ ActionExecutor::action_hub_callback(const plansys2_msgs::msg::ActionExecution::S break; default: RCLCPP_ERROR( - node_->get_logger(), "Msg %d type not recognized in %s executor requester", - msg->type, action_.c_str()); + node_->get_logger(), "Msg %d type not recognized in %s executor requester", msg->type, + action_.c_str()); break; } } -void -ActionExecutor::confirm_performer(const std::string & node_id) +void ActionExecutor::confirm_performer(const std::string & node_id) { plansys2_msgs::msg::ActionExecution msg; msg.type = plansys2_msgs::msg::ActionExecution::CONFIRM; @@ -123,8 +119,7 @@ ActionExecutor::confirm_performer(const std::string & node_id) action_hub_pub_->publish(msg); } -void -ActionExecutor::reject_performer(const std::string & node_id) +void ActionExecutor::reject_performer(const std::string & node_id) { plansys2_msgs::msg::ActionExecution msg; msg.type = plansys2_msgs::msg::ActionExecution::REJECT; @@ -135,8 +130,7 @@ ActionExecutor::reject_performer(const std::string & node_id) action_hub_pub_->publish(msg); } -void -ActionExecutor::request_for_performers() +void ActionExecutor::request_for_performers() { plansys2_msgs::msg::ActionExecution msg; msg.type = plansys2_msgs::msg::ActionExecution::REQUEST; @@ -147,8 +141,7 @@ ActionExecutor::request_for_performers() action_hub_pub_->publish(msg); } -BT::NodeStatus -ActionExecutor::get_status() +BT::NodeStatus ActionExecutor::get_status() { switch (state_) { case IDLE: @@ -170,14 +163,9 @@ ActionExecutor::get_status() } } -bool -ActionExecutor::is_finished() -{ - return state_ == SUCCESS || state_ == FAILURE; -} +bool ActionExecutor::is_finished() { return state_ == SUCCESS || state_ == FAILURE; } -BT::NodeStatus -ActionExecutor::tick(const rclcpp::Time & now) +BT::NodeStatus ActionExecutor::tick(const rclcpp::Time & now) { switch (state_) { case IDLE: @@ -190,20 +178,17 @@ ActionExecutor::tick(const rclcpp::Time & now) feedback_ = ""; request_for_performers(); - waiting_timer_ = node_->create_wall_timer( - 1s, std::bind(&ActionExecutor::wait_timeout, this)); + waiting_timer_ = node_->create_wall_timer(1s, std::bind(&ActionExecutor::wait_timeout, this)); break; - case DEALING: - { - auto time_since_dealing = (node_->now() - state_time_).seconds(); - if (time_since_dealing > 30.0) { - RCLCPP_ERROR( - node_->get_logger(), - "Aborting %s. Timeout after requesting for 30 seconds", action_.c_str()); - state_ = FAILURE; - } + case DEALING: { + auto time_since_dealing = (node_->now() - state_time_).seconds(); + if (time_since_dealing > 30.0) { + RCLCPP_ERROR( + node_->get_logger(), "Aborting %s. Timeout after requesting for 30 seconds", + action_.c_str()); + state_ = FAILURE; } - break; + } break; case RUNNING: break; @@ -218,8 +203,7 @@ ActionExecutor::tick(const rclcpp::Time & now) return get_status(); } -void -ActionExecutor::cancel() +void ActionExecutor::cancel() { state_ = CANCELLED; plansys2_msgs::msg::ActionExecution msg; @@ -231,26 +215,24 @@ ActionExecutor::cancel() action_hub_pub_->publish(msg); } -std::string -ActionExecutor::get_name(const std::string & action_expr) +std::string ActionExecutor::get_name(const std::string & action_expr) { std::string working_action_expr = parser::pddl::getReducedString(action_expr); working_action_expr.erase(0, 1); // remove initial ( - working_action_expr.pop_back(); // remove last ) + working_action_expr.pop_back(); // remove last ) size_t delim = working_action_expr.find(" "); return working_action_expr.substr(0, delim); } -std::vector -ActionExecutor::get_params(const std::string & action_expr) +std::vector ActionExecutor::get_params(const std::string & action_expr) { std::vector ret; std::string working_action_expr = parser::pddl::getReducedString(action_expr); working_action_expr.erase(0, 1); // remove initial ( - working_action_expr.pop_back(); // remove last ) + working_action_expr.pop_back(); // remove last ) size_t delim = working_action_expr.find(" "); @@ -268,8 +250,7 @@ ActionExecutor::get_params(const std::string & action_expr) return ret; } -void -ActionExecutor::wait_timeout() +void ActionExecutor::wait_timeout() { RCLCPP_WARN(node_->get_logger(), "No action performer for %s. retrying", action_.c_str()); request_for_performers(); diff --git a/plansys2_executor/src/plansys2_executor/ActionExecutorClient.cpp b/plansys2_executor/src/plansys2_executor/ActionExecutorClient.cpp index 300a7e29..f5c85a19 100644 --- a/plansys2_executor/src/plansys2_executor/ActionExecutorClient.cpp +++ b/plansys2_executor/src/plansys2_executor/ActionExecutorClient.cpp @@ -12,28 +12,23 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "plansys2_executor/ActionExecutorClient.hpp" + #include +#include #include -#include "lifecycle_msgs/msg/transition.hpp" #include "lifecycle_msgs/msg/state.hpp" - +#include "lifecycle_msgs/msg/transition.hpp" #include "plansys2_msgs/msg/action_execution_info.hpp" -#include "plansys2_executor/ActionExecutorClient.hpp" - namespace plansys2 { - using namespace std::chrono_literals; ActionExecutorClient::ActionExecutorClient( - const std::string & node_name, - const std::chrono::nanoseconds & rate) -: CascadeLifecycleNode(node_name), - rate_(rate), - commited_(false) + const std::string & node_name, const std::chrono::nanoseconds & rate) +: CascadeLifecycleNode(node_name), rate_(rate), commited_(false) { declare_parameter("action_name", ""); declare_parameter>( @@ -46,22 +41,19 @@ ActionExecutorClient::ActionExecutorClient( status_.node_name = get_name(); } -using CallbackReturnT = - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; +using CallbackReturnT = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; using std::placeholders::_1; -CallbackReturnT -ActionExecutorClient::on_configure(const rclcpp_lifecycle::State & state) +CallbackReturnT ActionExecutorClient::on_configure(const rclcpp_lifecycle::State & state) { status_pub_ = create_publisher( "performers_status", rclcpp::QoS(100).reliable()); status_pub_->on_activate(); - hearbeat_pub_ = create_wall_timer( - 1s, [this]() { - status_.status_stamp = now(); - status_pub_->publish(status_); - }); + hearbeat_pub_ = create_wall_timer(1s, [this]() { + status_.status_stamp = now(); + status_pub_->publish(status_); + }); if (!get_parameter("action_name", action_managed_)) { RCLCPP_ERROR(get_logger(), "action_name parameter not set"); @@ -93,21 +85,18 @@ ActionExecutorClient::on_configure(const rclcpp_lifecycle::State & state) return CallbackReturnT::SUCCESS; } -CallbackReturnT -ActionExecutorClient::on_activate(const rclcpp_lifecycle::State & state) +CallbackReturnT ActionExecutorClient::on_activate(const rclcpp_lifecycle::State & state) { status_.state = plansys2_msgs::msg::ActionPerformerStatus::RUNNING; status_.status_stamp = now(); - timer_ = create_wall_timer( - rate_, std::bind(&ActionExecutorClient::do_work, this)); + timer_ = create_wall_timer(rate_, std::bind(&ActionExecutorClient::do_work, this)); -// do_work(); + // do_work(); return CallbackReturnT::SUCCESS; } -CallbackReturnT -ActionExecutorClient::on_deactivate(const rclcpp_lifecycle::State & state) +CallbackReturnT ActionExecutorClient::on_deactivate(const rclcpp_lifecycle::State & state) { status_.state = plansys2_msgs::msg::ActionPerformerStatus::READY; status_.status_stamp = now(); @@ -116,22 +105,22 @@ ActionExecutorClient::on_deactivate(const rclcpp_lifecycle::State & state) return CallbackReturnT::SUCCESS; } -void -ActionExecutorClient::action_hub_callback(const plansys2_msgs::msg::ActionExecution::SharedPtr msg) +void ActionExecutorClient::action_hub_callback( + const plansys2_msgs::msg::ActionExecution::SharedPtr msg) { switch (msg->type) { case plansys2_msgs::msg::ActionExecution::REQUEST: - if (get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE && - !commited_ && should_execute(msg->action, msg->arguments)) - { + if ( + get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE && + !commited_ && should_execute(msg->action, msg->arguments)) { commited_ = true; send_response(msg); } break; case plansys2_msgs::msg::ActionExecution::CONFIRM: - if (get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE && - commited_ && msg->node_id == get_name()) - { + if ( + get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE && + commited_ && msg->node_id == get_name()) { current_arguments_ = msg->arguments; trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE); commited_ = false; @@ -143,9 +132,9 @@ ActionExecutorClient::action_hub_callback(const plansys2_msgs::msg::ActionExecut } break; case plansys2_msgs::msg::ActionExecution::CANCEL: - if (get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE && - msg->node_id == get_name()) - { + if ( + get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE && + msg->node_id == get_name()) { trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE); } break; @@ -155,14 +144,12 @@ ActionExecutorClient::action_hub_callback(const plansys2_msgs::msg::ActionExecut break; default: RCLCPP_ERROR( - get_logger(), "Msg %d type not recognized in %s executor performer", - msg->type, get_name()); + get_logger(), "Msg %d type not recognized in %s executor performer", msg->type, get_name()); break; } } -bool -ActionExecutorClient::should_execute( +bool ActionExecutorClient::should_execute( const std::string & action, const std::vector & args) { if (action != action_managed_) { @@ -172,14 +159,13 @@ ActionExecutorClient::should_execute( if (!specialized_arguments_.empty()) { if (specialized_arguments_.size() != args.size()) { RCLCPP_WARN( - get_logger(), "current and specialized arguments length doesn't match %zu %zu", - args.size(), specialized_arguments_.size()); + get_logger(), "current and specialized arguments length doesn't match %zu %zu", args.size(), + specialized_arguments_.size()); } for (size_t i = 0; i < specialized_arguments_.size() && i < args.size(); i++) { - if (specialized_arguments_[i] != "" && args[i] != "" && - specialized_arguments_[i] != args[i]) - { + if ( + specialized_arguments_[i] != "" && args[i] != "" && specialized_arguments_[i] != args[i]) { return false; } } @@ -188,9 +174,7 @@ ActionExecutorClient::should_execute( return true; } -void -ActionExecutorClient::send_response( - const plansys2_msgs::msg::ActionExecution::SharedPtr msg) +void ActionExecutorClient::send_response(const plansys2_msgs::msg::ActionExecution::SharedPtr msg) { plansys2_msgs::msg::ActionExecution msg_resp(*msg); msg_resp.type = plansys2_msgs::msg::ActionExecution::RESPONSE; @@ -199,8 +183,7 @@ ActionExecutorClient::send_response( action_hub_pub_->publish(msg_resp); } -void -ActionExecutorClient::send_feedback(float completion, const std::string & status) +void ActionExecutorClient::send_feedback(float completion, const std::string & status) { plansys2_msgs::msg::ActionExecution msg_resp; msg_resp.type = plansys2_msgs::msg::ActionExecution::FEEDBACK; @@ -213,8 +196,7 @@ ActionExecutorClient::send_feedback(float completion, const std::string & status action_hub_pub_->publish(msg_resp); } -void -ActionExecutorClient::finish(bool success, float completion, const std::string & status) +void ActionExecutorClient::finish(bool success, float completion, const std::string & status) { if (get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE); diff --git a/plansys2_executor/src/plansys2_executor/ComputeBT.cpp b/plansys2_executor/src/plansys2_executor/ComputeBT.cpp index 98628d8d..8a3adb64 100644 --- a/plansys2_executor/src/plansys2_executor/ComputeBT.cpp +++ b/plansys2_executor/src/plansys2_executor/ComputeBT.cpp @@ -47,7 +47,6 @@ namespace plansys2 { - ComputeBT::ComputeBT() : rclcpp_lifecycle::LifecycleNode("compute_bt"), bt_builder_loader_("plansys2_executor", "plansys2::BTBuilder") @@ -68,8 +67,7 @@ ComputeBT::ComputeBT() auto action_timeouts_actions = this->get_parameter("action_timeouts.actions").as_string_array(); for (auto action : action_timeouts_actions) { this->declare_parameter( - "action_timeouts." + action + ".duration_overrun_percentage", - 0.0); + "action_timeouts." + action + ".duration_overrun_percentage", 0.0); } #ifdef ZMQ_FOUND @@ -80,32 +78,26 @@ ComputeBT::ComputeBT() #endif compute_bt_srv_ = create_service( - "compute_bt", - std::bind( - &ComputeBT::computeBTCallback, - this, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3)); + "compute_bt", std::bind( + &ComputeBT::computeBTCallback, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3)); } -using CallbackReturnT = - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; +using CallbackReturnT = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; -CallbackReturnT -ComputeBT::on_configure(const rclcpp_lifecycle::State & state) +CallbackReturnT ComputeBT::on_configure(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "[%s] Configuring...", get_name()); - auto action_bt_xml_filename = - this->get_parameter("action_bt_xml_filename").as_string(); + auto action_bt_xml_filename = this->get_parameter("action_bt_xml_filename").as_string(); if (action_bt_xml_filename.empty()) { - action_bt_xml_filename = - ament_index_cpp::get_package_share_directory("plansys2_executor") + - "/behavior_trees/plansys2_action_bt.xml"; + action_bt_xml_filename = ament_index_cpp::get_package_share_directory("plansys2_executor") + + "/behavior_trees/plansys2_action_bt.xml"; } std::ifstream action_bt_ifs(action_bt_xml_filename); if (!action_bt_ifs) { - RCLCPP_ERROR_STREAM(get_logger(), "Error openning [" << action_bt_xml_filename << "]"); + RCLCPP_ERROR_STREAM(get_logger(), "Error opening [" << action_bt_xml_filename << "]"); return CallbackReturnT::FAILURE; } @@ -122,26 +114,22 @@ ComputeBT::on_configure(const rclcpp_lifecycle::State & state) std::ifstream start_action_bt_ifs(start_action_bt_xml_filename); if (!start_action_bt_ifs) { - RCLCPP_ERROR_STREAM( - get_logger(), "Error openning [" << start_action_bt_xml_filename << "]"); + RCLCPP_ERROR_STREAM(get_logger(), "Error opening [" << start_action_bt_xml_filename << "]"); return CallbackReturnT::FAILURE; } start_action_bt_xml_.assign( std::istreambuf_iterator(start_action_bt_ifs), std::istreambuf_iterator()); - auto end_action_bt_xml_filename = - this->get_parameter("end_action_bt_xml_filename").as_string(); + auto end_action_bt_xml_filename = this->get_parameter("end_action_bt_xml_filename").as_string(); if (end_action_bt_xml_filename.empty()) { - end_action_bt_xml_filename = - ament_index_cpp::get_package_share_directory("plansys2_executor") + - "/behavior_trees/plansys2_end_action_bt.xml"; + end_action_bt_xml_filename = ament_index_cpp::get_package_share_directory("plansys2_executor") + + "/behavior_trees/plansys2_end_action_bt.xml"; } std::ifstream end_action_bt_ifs(end_action_bt_xml_filename); if (!end_action_bt_ifs) { - RCLCPP_ERROR_STREAM( - get_logger(), "Error openning [" << end_action_bt_xml_filename << "]"); + RCLCPP_ERROR_STREAM(get_logger(), "Error opening [" << end_action_bt_xml_filename << "]"); return CallbackReturnT::FAILURE; } @@ -162,8 +150,7 @@ ComputeBT::on_configure(const rclcpp_lifecycle::State & state) return CallbackReturnT::SUCCESS; } -CallbackReturnT -ComputeBT::on_activate(const rclcpp_lifecycle::State & state) +CallbackReturnT ComputeBT::on_activate(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "[%s] Activating...", get_name()); dotgraph_pub_->on_activate(); @@ -171,8 +158,7 @@ ComputeBT::on_activate(const rclcpp_lifecycle::State & state) return CallbackReturnT::SUCCESS; } -CallbackReturnT -ComputeBT::on_deactivate(const rclcpp_lifecycle::State & state) +CallbackReturnT ComputeBT::on_deactivate(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "[%s] Deactivating...", get_name()); dotgraph_pub_->on_deactivate(); @@ -180,8 +166,7 @@ ComputeBT::on_deactivate(const rclcpp_lifecycle::State & state) return CallbackReturnT::SUCCESS; } -CallbackReturnT -ComputeBT::on_cleanup(const rclcpp_lifecycle::State & state) +CallbackReturnT ComputeBT::on_cleanup(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "[%s] Cleaning up...", get_name()); dotgraph_pub_.reset(); @@ -189,8 +174,7 @@ ComputeBT::on_cleanup(const rclcpp_lifecycle::State & state) return CallbackReturnT::SUCCESS; } -CallbackReturnT -ComputeBT::on_shutdown(const rclcpp_lifecycle::State & state) +CallbackReturnT ComputeBT::on_shutdown(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "[%s] Shutting down...", get_name()); dotgraph_pub_.reset(); @@ -198,15 +182,13 @@ ComputeBT::on_shutdown(const rclcpp_lifecycle::State & state) return CallbackReturnT::SUCCESS; } -CallbackReturnT -ComputeBT::on_error(const rclcpp_lifecycle::State & state) +CallbackReturnT ComputeBT::on_error(const rclcpp_lifecycle::State & state) { RCLCPP_ERROR(get_logger(), "[%s] Error transition", get_name()); return CallbackReturnT::SUCCESS; } -void -ComputeBT::computeBTCallback( +void ComputeBT::computeBTCallback( const std::shared_ptr request_header, const std::shared_ptr request, const std::shared_ptr response) @@ -238,9 +220,10 @@ ComputeBT::computeBTCallback( bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} - }); - + while (!finish) { + exe.spin_some(); + } + }); domain_node_->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); problem_node_->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); @@ -271,19 +254,18 @@ ComputeBT::computeBTCallback( (*action_map)[index] = ActionExecutionInfo(); (*action_map)[index].action_executor = ActionExecutor::make_shared(plan_item.action, shared_from_this()); - (*action_map)[index].durative_action_info = - domain_client_->getDurativeAction( + (*action_map)[index].durative_action_info = domain_client_->getDurativeAction( get_action_name(plan_item.action), get_action_params(plan_item.action)); (*action_map)[index].duration = plan_item.duration; std::string action_name = (*action_map)[index].durative_action_info->name; - if (std::find( - action_timeout_actions.begin(), action_timeout_actions.end(), - action_name) != action_timeout_actions.end() && - this->has_parameter("action_timeouts." + action_name + ".duration_overrun_percentage")) - { - (*action_map)[index].duration_overrun_percentage = this->get_parameter( - "action_timeouts." + action_name + ".duration_overrun_percentage").as_double(); + if ( + std::find(action_timeout_actions.begin(), action_timeout_actions.end(), action_name) != + action_timeout_actions.end() && + this->has_parameter("action_timeouts." + action_name + ".duration_overrun_percentage")) { + (*action_map)[index].duration_overrun_percentage = + this->get_parameter("action_timeouts." + action_name + ".duration_overrun_percentage") + .as_double(); } RCLCPP_INFO( get_logger(), "Action %s timeout percentage %f", action_name.c_str(), @@ -352,9 +334,7 @@ ComputeBT::computeBTCallback( get_name(), publisher_port, server_port, max_msgs_per_second); try { publisher_zmq.reset( - new BT::PublisherZMQ( - tree, max_msgs_per_second, publisher_port, - server_port)); + new BT::PublisherZMQ(tree, max_msgs_per_second, publisher_port, server_port)); } catch (const BT::LogicError & exc) { RCLCPP_ERROR(get_logger(), "ZMQ error: %s", exc.what()); } @@ -367,8 +347,7 @@ ComputeBT::computeBTCallback( response->success = true; } -std::string -ComputeBT::getProblem(const std::string & filename) const +std::string ComputeBT::getProblem(const std::string & filename) const { std::string ret; std::ifstream file(filename); @@ -381,8 +360,7 @@ ComputeBT::getProblem(const std::string & filename) const return ret; } -void -ComputeBT::savePlan(const plansys2_msgs::msg::Plan & plan, const std::string & filename) const +void ComputeBT::savePlan(const plansys2_msgs::msg::Plan & plan, const std::string & filename) const { std::ofstream file(filename + "_plan.pddl"); if (file.is_open()) { @@ -395,8 +373,7 @@ ComputeBT::savePlan(const plansys2_msgs::msg::Plan & plan, const std::string & f } } -void -ComputeBT::saveBT(const std::string & bt_xml, const std::string & filename) const +void ComputeBT::saveBT(const std::string & bt_xml, const std::string & filename) const { std::ofstream file(filename + "_bt.xml"); if (file.is_open()) { @@ -407,8 +384,7 @@ ComputeBT::saveBT(const std::string & bt_xml, const std::string & filename) cons } } -void -ComputeBT::saveDotGraph(const std::string & dotgraph, const std::string & filename) const +void ComputeBT::saveDotGraph(const std::string & dotgraph, const std::string & filename) const { std::ofstream file(filename + "_graph.dot"); if (file.is_open()) { diff --git a/plansys2_executor/src/plansys2_executor/ExecutorClient.cpp b/plansys2_executor/src/plansys2_executor/ExecutorClient.cpp index 46237259..a4652823 100644 --- a/plansys2_executor/src/plansys2_executor/ExecutorClient.cpp +++ b/plansys2_executor/src/plansys2_executor/ExecutorClient.cpp @@ -12,18 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "plansys2_executor/ExecutorClient.hpp" + #include +#include +#include #include #include -#include -#include "plansys2_executor/ExecutorClient.hpp" #include "plansys2_msgs/msg/action_execution_info.hpp" namespace plansys2 { - using namespace std::chrono_literals; using namespace std::placeholders; @@ -35,8 +35,8 @@ ExecutorClient::ExecutorClient() createActionClient(); - get_ordered_sub_goals_client_ = node_->create_client( - "executor/get_ordered_sub_goals"); + get_ordered_sub_goals_client_ = + node_->create_client("executor/get_ordered_sub_goals"); get_plan_client_ = node_->create_client("executor/get_plan"); } @@ -46,13 +46,12 @@ ExecutorClient::ExecutorClient(const std::string & node_name) createActionClient(); - get_ordered_sub_goals_client_ = node_->create_client( - "executor/get_ordered_sub_goals"); + get_ordered_sub_goals_client_ = + node_->create_client("executor/get_ordered_sub_goals"); get_plan_client_ = node_->create_client("executor/get_plan"); } -void -ExecutorClient::createActionClient() +void ExecutorClient::createActionClient() { action_client_ = rclcpp_action::create_client(node_, "execute_plan"); @@ -61,8 +60,7 @@ ExecutorClient::createActionClient() } } -bool -ExecutorClient::start_plan_execution(const plansys2_msgs::msg::Plan & plan) +bool ExecutorClient::start_plan_execution(const plansys2_msgs::msg::Plan & plan) { if (!executing_plan_) { createActionClient(); @@ -79,8 +77,7 @@ ExecutorClient::start_plan_execution(const plansys2_msgs::msg::Plan & plan) return false; } -bool -ExecutorClient::execute_and_check_plan() +bool ExecutorClient::execute_and_check_plan() { if (rclcpp::ok() && !goal_result_available_) { rclcpp::spin_some(node_); @@ -93,8 +90,7 @@ ExecutorClient::execute_and_check_plan() switch (result_.code) { case rclcpp_action::ResultCode::SUCCEEDED: if (result_.result == nullptr) { - RCLCPP_WARN( - node_->get_logger(), "Plan failed due to a nullptr in the result"); + RCLCPP_WARN(node_->get_logger(), "Plan failed due to a nullptr in the result"); } else if (result_.result->success) { RCLCPP_INFO(node_->get_logger(), "Plan Succeeded"); } else { @@ -104,39 +100,26 @@ ExecutorClient::execute_and_check_plan() case plansys2_msgs::msg::ActionExecutionInfo::SUCCEEDED: RCLCPP_WARN_STREAM( node_->get_logger(), - "Action: " << - msg.action_full_name << - " succeeded with message_status: " << - msg.message_status); + "Action: " << msg.action_full_name + << " succeeded with message_status: " << msg.message_status); break; case plansys2_msgs::msg::ActionExecutionInfo::FAILED: RCLCPP_ERROR_STREAM( node_->get_logger(), - "Action: " << - msg.action_full_name << - " failed with message_status: " << - msg.message_status); + "Action: " << msg.action_full_name + << " failed with message_status: " << msg.message_status); break; case plansys2_msgs::msg::ActionExecutionInfo::NOT_EXECUTED: RCLCPP_WARN_STREAM( - node_->get_logger(), - "Action: " << - msg.action_full_name << - " was not executed"); + node_->get_logger(), "Action: " << msg.action_full_name << " was not executed"); break; case plansys2_msgs::msg::ActionExecutionInfo::CANCELLED: RCLCPP_WARN_STREAM( - node_->get_logger(), - "Action: " << - msg.action_full_name << - " was cancelled"); + node_->get_logger(), "Action: " << msg.action_full_name << " was cancelled"); break; case plansys2_msgs::msg::ActionExecutionInfo::EXECUTING: RCLCPP_WARN_STREAM( - node_->get_logger(), - "Action: " << - msg.action_full_name << - " was executing"); + node_->get_logger(), "Action: " << msg.action_full_name << " was executing"); } } } @@ -160,27 +143,22 @@ ExecutorClient::execute_and_check_plan() return false; // Plan finished } - -bool -ExecutorClient::on_new_goal_received(const plansys2_msgs::msg::Plan & plan) +bool ExecutorClient::on_new_goal_received(const plansys2_msgs::msg::Plan & plan) { auto goal = ExecutePlan::Goal(); goal.plan = plan; auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); - send_goal_options.feedback_callback = - std::bind(&ExecutorClient::feedback_callback, this, _1, _2); + send_goal_options.feedback_callback = std::bind(&ExecutorClient::feedback_callback, this, _1, _2); - send_goal_options.result_callback = - std::bind(&ExecutorClient::result_callback, this, _1); + send_goal_options.result_callback = std::bind(&ExecutorClient::result_callback, this, _1); auto future_goal_handle = action_client_->async_send_goal(goal, send_goal_options); - if (rclcpp::spin_until_future_complete( - node_->get_node_base_interface(), future_goal_handle, 3s) != - rclcpp::FutureReturnCode::SUCCESS) - { + if ( + rclcpp::spin_until_future_complete(node_->get_node_base_interface(), future_goal_handle, 3s) != + rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(node_->get_logger(), "send_goal failed"); return false; } @@ -194,8 +172,7 @@ ExecutorClient::on_new_goal_received(const plansys2_msgs::msg::Plan & plan) return true; } -bool -ExecutorClient::should_cancel_goal() +bool ExecutorClient::should_cancel_goal() { if (!executing_plan_) { return false; @@ -208,18 +185,14 @@ ExecutorClient::should_cancel_goal() status == action_msgs::msg::GoalStatus::STATUS_EXECUTING; } -void -ExecutorClient::cancel_plan_execution() +void ExecutorClient::cancel_plan_execution() { if (should_cancel_goal()) { auto future_cancel = action_client_->async_cancel_goal(goal_handle_); - if (rclcpp::spin_until_future_complete( - node_->get_node_base_interface(), future_cancel, 3s) != - rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_ERROR( - node_->get_logger(), - "Failed to cancel action server for execute_plan"); + if ( + rclcpp::spin_until_future_complete(node_->get_node_base_interface(), future_cancel, 3s) != + rclcpp::FutureReturnCode::SUCCESS) { + RCLCPP_ERROR(node_->get_logger(), "Failed to cancel action server for execute_plan"); } } @@ -236,18 +209,17 @@ std::vector ExecutorClient::getOrderedSubGoals() return ret; } RCLCPP_ERROR_STREAM( - node_->get_logger(), - get_ordered_sub_goals_client_->get_service_name() << - " service client: waiting for service to appear..."); + node_->get_logger(), get_ordered_sub_goals_client_->get_service_name() + << " service client: waiting for service to appear..."); } auto request = std::make_shared(); auto future_result = get_ordered_sub_goals_client_->async_send_request(request); - if (rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != - rclcpp::FutureReturnCode::SUCCESS) - { + if ( + rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != + rclcpp::FutureReturnCode::SUCCESS) { return ret; } @@ -257,9 +229,8 @@ std::vector ExecutorClient::getOrderedSubGoals() ret = result.sub_goals; } else { RCLCPP_INFO_STREAM( - node_->get_logger(), - get_ordered_sub_goals_client_->get_service_name() << ": " << - result.error_info); + node_->get_logger(), get_ordered_sub_goals_client_->get_service_name() + << ": " << result.error_info); } return ret; @@ -272,18 +243,17 @@ std::optional ExecutorClient::getPlan() return {}; } RCLCPP_ERROR_STREAM( - node_->get_logger(), - get_plan_client_->get_service_name() << - " service client: waiting for service to appear..."); + node_->get_logger(), get_plan_client_->get_service_name() << " service client: waiting " + "for service to appear..."); } auto request = std::make_shared(); auto future_result = get_plan_client_->async_send_request(request); - if (rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != - rclcpp::FutureReturnCode::SUCCESS) - { + if ( + rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(1)) != + rclcpp::FutureReturnCode::SUCCESS) { return {}; } @@ -293,31 +263,26 @@ std::optional ExecutorClient::getPlan() return result.plan; } else { RCLCPP_ERROR_STREAM( - node_->get_logger(), - get_plan_client_->get_service_name() << ": " << - result.error_info); + node_->get_logger(), get_plan_client_->get_service_name() << ": " << result.error_info); return {}; } } -void -ExecutorClient::feedback_callback( +void ExecutorClient::feedback_callback( GoalHandleExecutePlan::SharedPtr goal_handle, const std::shared_ptr feedback) { feedback_ = *feedback; } -void -ExecutorClient::result_callback(const GoalHandleExecutePlan::WrappedResult & result) +void ExecutorClient::result_callback(const GoalHandleExecutePlan::WrappedResult & result) { goal_result_available_ = true; result_ = result; feedback_ = ExecutePlan::Feedback(); } -std::optional -ExecutorClient::getResult() +std::optional ExecutorClient::getResult() { if (result_.result != nullptr) { return *result_.result; diff --git a/plansys2_executor/src/plansys2_executor/ExecutorNode.cpp b/plansys2_executor/src/plansys2_executor/ExecutorNode.cpp index 8d6743b2..8564aeee 100644 --- a/plansys2_executor/src/plansys2_executor/ExecutorNode.cpp +++ b/plansys2_executor/src/plansys2_executor/ExecutorNode.cpp @@ -12,51 +12,47 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "plansys2_executor/ExecutorNode.hpp" #include -#include -#include -#include +#include #include +#include #include +#include #include +#include #include -#include "plansys2_executor/ExecutorNode.hpp" -#include "plansys2_executor/ActionExecutor.hpp" -#include "plansys2_executor/BTBuilder.hpp" -#include "plansys2_problem_expert/Utils.hpp" -#include "plansys2_pddl_parser/Utils.h" - -#include "lifecycle_msgs/msg/state.hpp" -#include "plansys2_msgs/msg/action_execution_info.hpp" -#include "plansys2_msgs/msg/plan.hpp" - #include "ament_index_cpp/get_package_share_directory.hpp" - #include "behaviortree_cpp_v3/behavior_tree.h" +#include "behaviortree_cpp_v3/blackboard.h" #include "behaviortree_cpp_v3/bt_factory.h" #include "behaviortree_cpp_v3/utils/shared_library.h" -#include "behaviortree_cpp_v3/blackboard.h" +#include "lifecycle_msgs/msg/state.hpp" +#include "plansys2_executor/ActionExecutor.hpp" +#include "plansys2_executor/BTBuilder.hpp" +#include "plansys2_msgs/msg/action_execution_info.hpp" +#include "plansys2_msgs/msg/plan.hpp" +#include "plansys2_pddl_parser/Utils.h" +#include "plansys2_problem_expert/Utils.hpp" #ifdef ZMQ_FOUND #include #endif -#include "plansys2_executor/behavior_tree/execute_action_node.hpp" -#include "plansys2_executor/behavior_tree/wait_action_node.hpp" +#include "plansys2_executor/behavior_tree/apply_atend_effect_node.hpp" +#include "plansys2_executor/behavior_tree/apply_atstart_effect_node.hpp" #include "plansys2_executor/behavior_tree/check_action_node.hpp" -#include "plansys2_executor/behavior_tree/wait_atstart_req_node.hpp" -#include "plansys2_executor/behavior_tree/check_overall_req_node.hpp" #include "plansys2_executor/behavior_tree/check_atend_req_node.hpp" +#include "plansys2_executor/behavior_tree/check_overall_req_node.hpp" #include "plansys2_executor/behavior_tree/check_timeout_node.hpp" -#include "plansys2_executor/behavior_tree/apply_atstart_effect_node.hpp" -#include "plansys2_executor/behavior_tree/apply_atend_effect_node.hpp" +#include "plansys2_executor/behavior_tree/execute_action_node.hpp" +#include "plansys2_executor/behavior_tree/wait_action_node.hpp" +#include "plansys2_executor/behavior_tree/wait_atstart_req_node.hpp" namespace plansys2 { - using ExecutePlan = plansys2_msgs::action::ExecutePlan; using namespace std::chrono_literals; @@ -74,12 +70,12 @@ ExecutorNode::ExecutorNode() this->declare_parameter("enable_dotgraph_legend", true); this->declare_parameter("print_graph", false); this->declare_parameter("action_timeouts.actions", std::vector{}); - // Declaring individual action parameters so they can be queried on the command line + // Declaring individual action parameters so they can be queried on the + // command line auto action_timeouts_actions = this->get_parameter("action_timeouts.actions").as_string_array(); for (auto action : action_timeouts_actions) { this->declare_parameter( - "action_timeouts." + action + ".duration_overrun_percentage", - 0.0); + "action_timeouts." + action + ".duration_overrun_percentage", 0.0); } #ifdef ZMQ_FOUND @@ -90,11 +86,8 @@ ExecutorNode::ExecutorNode() #endif execute_plan_action_server_ = rclcpp_action::create_server( - this->get_node_base_interface(), - this->get_node_clock_interface(), - this->get_node_logging_interface(), - this->get_node_waitables_interface(), - "execute_plan", + this->get_node_base_interface(), this->get_node_clock_interface(), + this->get_node_logging_interface(), this->get_node_waitables_interface(), "execute_plan", std::bind(&ExecutorNode::handle_goal, this, _1, _2), std::bind(&ExecutorNode::handle_cancel, this, _1), std::bind(&ExecutorNode::handle_accepted, this, _1)); @@ -102,24 +95,18 @@ ExecutorNode::ExecutorNode() get_ordered_sub_goals_service_ = create_service( "executor/get_ordered_sub_goals", std::bind( - &ExecutorNode::get_ordered_sub_goals_service_callback, - this, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3)); + &ExecutorNode::get_ordered_sub_goals_service_callback, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3)); get_plan_service_ = create_service( - "executor/get_plan", - std::bind( - &ExecutorNode::get_plan_service_callback, - this, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3)); + "executor/get_plan", std::bind( + &ExecutorNode::get_plan_service_callback, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3)); } +using CallbackReturnT = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; -using CallbackReturnT = - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - -CallbackReturnT -ExecutorNode::on_configure(const rclcpp_lifecycle::State & state) +CallbackReturnT ExecutorNode::on_configure(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "[%s] Configuring...", get_name()); @@ -133,7 +120,7 @@ ExecutorNode::on_configure(const rclcpp_lifecycle::State & state) std::ifstream action_bt_ifs(default_action_bt_xml_filename); if (!action_bt_ifs) { - RCLCPP_ERROR_STREAM(get_logger(), "Error openning [" << default_action_bt_xml_filename << "]"); + RCLCPP_ERROR_STREAM(get_logger(), "Error opening [" << default_action_bt_xml_filename << "]"); return CallbackReturnT::FAILURE; } @@ -151,7 +138,7 @@ ExecutorNode::on_configure(const rclcpp_lifecycle::State & state) std::ifstream start_action_bt_ifs(default_start_action_bt_xml_filename); if (!start_action_bt_ifs) { RCLCPP_ERROR_STREAM( - get_logger(), "Error openning [" << default_start_action_bt_xml_filename << "]"); + get_logger(), "Error opening [" << default_start_action_bt_xml_filename << "]"); return CallbackReturnT::FAILURE; } @@ -169,7 +156,7 @@ ExecutorNode::on_configure(const rclcpp_lifecycle::State & state) std::ifstream end_action_bt_ifs(default_end_action_bt_xml_filename); if (!end_action_bt_ifs) { RCLCPP_ERROR_STREAM( - get_logger(), "Error openning [" << default_end_action_bt_xml_filename << "]"); + get_logger(), "Error opening [" << default_end_action_bt_xml_filename << "]"); return CallbackReturnT::FAILURE; } @@ -177,8 +164,8 @@ ExecutorNode::on_configure(const rclcpp_lifecycle::State & state) std::istreambuf_iterator(end_action_bt_ifs), std::istreambuf_iterator()); dotgraph_pub_ = this->create_publisher("dot_graph", 1); - execution_info_pub_ = create_publisher( - "action_execution_info", 100); + execution_info_pub_ = + create_publisher("action_execution_info", 100); executing_plan_pub_ = create_publisher( "executing_plan", rclcpp::QoS(100).transient_local()); @@ -190,8 +177,7 @@ ExecutorNode::on_configure(const rclcpp_lifecycle::State & state) return CallbackReturnT::SUCCESS; } -CallbackReturnT -ExecutorNode::on_activate(const rclcpp_lifecycle::State & state) +CallbackReturnT ExecutorNode::on_activate(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "[%s] Activating...", get_name()); dotgraph_pub_->on_activate(); @@ -202,8 +188,7 @@ ExecutorNode::on_activate(const rclcpp_lifecycle::State & state) return CallbackReturnT::SUCCESS; } -CallbackReturnT -ExecutorNode::on_deactivate(const rclcpp_lifecycle::State & state) +CallbackReturnT ExecutorNode::on_deactivate(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "[%s] Deactivating...", get_name()); dotgraph_pub_->on_deactivate(); @@ -213,8 +198,7 @@ ExecutorNode::on_deactivate(const rclcpp_lifecycle::State & state) return CallbackReturnT::SUCCESS; } -CallbackReturnT -ExecutorNode::on_cleanup(const rclcpp_lifecycle::State & state) +CallbackReturnT ExecutorNode::on_cleanup(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "[%s] Cleaning up...", get_name()); dotgraph_pub_.reset(); @@ -224,8 +208,7 @@ ExecutorNode::on_cleanup(const rclcpp_lifecycle::State & state) return CallbackReturnT::SUCCESS; } -CallbackReturnT -ExecutorNode::on_shutdown(const rclcpp_lifecycle::State & state) +CallbackReturnT ExecutorNode::on_shutdown(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "[%s] Shutting down...", get_name()); dotgraph_pub_.reset(); @@ -235,16 +218,14 @@ ExecutorNode::on_shutdown(const rclcpp_lifecycle::State & state) return CallbackReturnT::SUCCESS; } -CallbackReturnT -ExecutorNode::on_error(const rclcpp_lifecycle::State & state) +CallbackReturnT ExecutorNode::on_error(const rclcpp_lifecycle::State & state) { RCLCPP_ERROR(get_logger(), "[%s] Error transition", get_name()); return CallbackReturnT::SUCCESS; } -void -ExecutorNode::get_ordered_sub_goals_service_callback( +void ExecutorNode::get_ordered_sub_goals_service_callback( const std::shared_ptr request_header, const std::shared_ptr request, const std::shared_ptr response) @@ -258,8 +239,7 @@ ExecutorNode::get_ordered_sub_goals_service_callback( } } -std::optional> -ExecutorNode::getOrderedSubGoals() +std::optional> ExecutorNode::getOrderedSubGoals() { if (!current_plan_.has_value()) { return {}; @@ -273,7 +253,7 @@ ExecutorNode::getOrderedSubGoals() std::vector unordered_subgoals = parser::pddl::getSubtreeIds(goal); // just in case some goals are already satisfied - for (auto it = unordered_subgoals.begin(); it != unordered_subgoals.end(); ) { + for (auto it = unordered_subgoals.begin(); it != unordered_subgoals.end();) { if (check(goal, local_predicates, local_functions, *it)) { plansys2_msgs::msg::Tree new_goal; parser::pddl::fromString(new_goal, "(and " + parser::pddl::toString(goal, (*it)) + ")"); @@ -285,13 +265,12 @@ ExecutorNode::getOrderedSubGoals() } for (const auto & plan_item : current_plan_.value().items) { - std::shared_ptr action = - domain_client_->getDurativeAction( + std::shared_ptr action = domain_client_->getDurativeAction( get_action_name(plan_item.action), get_action_params(plan_item.action)); apply(action->at_start_effects, local_predicates, local_functions); apply(action->at_end_effects, local_predicates, local_functions); - for (auto it = unordered_subgoals.begin(); it != unordered_subgoals.end(); ) { + for (auto it = unordered_subgoals.begin(); it != unordered_subgoals.end();) { if (check(goal, local_predicates, local_functions, *it)) { plansys2_msgs::msg::Tree new_goal; parser::pddl::fromString(new_goal, "(and " + parser::pddl::toString(goal, (*it)) + ")"); @@ -306,8 +285,7 @@ ExecutorNode::getOrderedSubGoals() return ordered_goals; } -void -ExecutorNode::get_plan_service_callback( +void ExecutorNode::get_plan_service_callback( const std::shared_ptr request_header, const std::shared_ptr request, const std::shared_ptr response) @@ -321,10 +299,8 @@ ExecutorNode::get_plan_service_callback( } } -rclcpp_action::GoalResponse -ExecutorNode::handle_goal( - const rclcpp_action::GoalUUID & uuid, - std::shared_ptr goal) +rclcpp_action::GoalResponse ExecutorNode::handle_goal( + const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal) { RCLCPP_DEBUG(this->get_logger(), "Received goal request with order"); @@ -334,8 +310,7 @@ ExecutorNode::handle_goal( return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } -rclcpp_action::CancelResponse -ExecutorNode::handle_cancel( +rclcpp_action::CancelResponse ExecutorNode::handle_cancel( const std::shared_ptr goal_handle) { RCLCPP_DEBUG(this->get_logger(), "Received request to cancel goal"); @@ -345,8 +320,7 @@ ExecutorNode::handle_cancel( return rclcpp_action::CancelResponse::ACCEPT; } -void -ExecutorNode::execute(const std::shared_ptr goal_handle) +void ExecutorNode::execute(const std::shared_ptr goal_handle) { auto feedback = std::make_shared(); auto result = std::make_shared(); @@ -376,19 +350,18 @@ ExecutorNode::execute(const std::shared_ptr goal_handle) (*action_map)[index] = ActionExecutionInfo(); (*action_map)[index].action_executor = ActionExecutor::make_shared(plan_item.action, shared_from_this()); - (*action_map)[index].durative_action_info = - domain_client_->getDurativeAction( + (*action_map)[index].durative_action_info = domain_client_->getDurativeAction( get_action_name(plan_item.action), get_action_params(plan_item.action)); (*action_map)[index].duration = plan_item.duration; std::string action_name = (*action_map)[index].durative_action_info->name; - if (std::find( - action_timeout_actions.begin(), action_timeout_actions.end(), - action_name) != action_timeout_actions.end() && - this->has_parameter("action_timeouts." + action_name + ".duration_overrun_percentage")) - { - (*action_map)[index].duration_overrun_percentage = this->get_parameter( - "action_timeouts." + action_name + ".duration_overrun_percentage").as_double(); + if ( + std::find(action_timeout_actions.begin(), action_timeout_actions.end(), action_name) != + action_timeout_actions.end() && + this->has_parameter("action_timeouts." + action_name + ".duration_overrun_percentage")) { + (*action_map)[index].duration_overrun_percentage = + this->get_parameter("action_timeouts." + action_name + ".duration_overrun_percentage") + .as_double(); } RCLCPP_INFO( get_logger(), "Action %s timeout percentage %f", action_name.c_str(), @@ -456,26 +429,24 @@ ExecutorNode::execute(const std::shared_ptr goal_handle) if (this->get_parameter("enable_groot_monitoring").as_bool()) { RCLCPP_DEBUG( get_logger(), - "[%s] Groot monitoring: Publisher port: %d, Server port: %d, Max msgs per second: %d", + "[%s] Groot monitoring: Publisher port: %d, Server port: %d, " + "Max msgs per second: %d", get_name(), publisher_port, server_port, max_msgs_per_second); try { publisher_zmq.reset( - new BT::PublisherZMQ( - tree, max_msgs_per_second, publisher_port, - server_port)); + new BT::PublisherZMQ(tree, max_msgs_per_second, publisher_port, server_port)); } catch (const BT::LogicError & exc) { RCLCPP_ERROR(get_logger(), "ZMQ error: %s", exc.what()); } } #endif - auto info_pub = create_wall_timer( - 1s, [this, &action_map]() { - auto msgs = get_feedback_info(action_map); - for (const auto & msg : msgs) { - execution_info_pub_->publish(msg); - } - }); + auto info_pub = create_wall_timer(1s, [this, &action_map]() { + auto msgs = get_feedback_info(action_map); + for (const auto & msg : msgs) { + execution_info_pub_->publish(msg); + } + }); rclcpp::Rate rate(10); auto status = BT::NodeStatus::RUNNING; @@ -491,8 +462,8 @@ ExecutorNode::execute(const std::shared_ptr goal_handle) feedback->action_execution_status = get_feedback_info(action_map); goal_handle->publish_feedback(feedback); - dotgraph_msg.data = bt_builder->get_dotgraph( - action_map, this->get_parameter("enable_dotgraph_legend").as_bool()); + dotgraph_msg.data = + bt_builder->get_dotgraph(action_map, this->get_parameter("enable_dotgraph_legend").as_bool()); dotgraph_pub_->publish(dotgraph_msg); rate.sleep(); @@ -507,8 +478,8 @@ ExecutorNode::execute(const std::shared_ptr goal_handle) RCLCPP_ERROR(get_logger(), "Executor BT finished with FAILURE state"); } - dotgraph_msg.data = bt_builder->get_dotgraph( - action_map, this->get_parameter("enable_dotgraph_legend").as_bool()); + dotgraph_msg.data = + bt_builder->get_dotgraph(action_map, this->get_parameter("enable_dotgraph_legend").as_bool()); dotgraph_pub_->publish(dotgraph_msg); result->success = status == BT::NodeStatus::SUCCESS; @@ -516,9 +487,9 @@ ExecutorNode::execute(const std::shared_ptr goal_handle) size_t i = 0; while (i < result->action_execution_status.size() && result->success) { - if (result->action_execution_status[i].status != - plansys2_msgs::msg::ActionExecutionInfo::SUCCEEDED) - { + if ( + result->action_execution_status[i].status != + plansys2_msgs::msg::ActionExecutionInfo::SUCCEEDED) { result->success = false; } i++; @@ -534,17 +505,14 @@ ExecutorNode::execute(const std::shared_ptr goal_handle) } } -void -ExecutorNode::handle_accepted(const std::shared_ptr goal_handle) +void ExecutorNode::handle_accepted(const std::shared_ptr goal_handle) { using namespace std::placeholders; std::thread{std::bind(&ExecutorNode::execute, this, _1), goal_handle}.detach(); } -std::vector -ExecutorNode::get_feedback_info( - std::shared_ptr> action_map) +std::vector ExecutorNode::get_feedback_info( + std::shared_ptr> action_map) { std::vector ret; @@ -595,8 +563,7 @@ ExecutorNode::get_feedback_info( return ret; } -void -ExecutorNode::print_execution_info( +void ExecutorNode::print_execution_info( std::shared_ptr> exec_info) { fprintf(stderr, "Execution info =====================\n"); diff --git a/plansys2_executor/src/plansys2_executor/behavior_tree/apply_atend_effect_node.cpp b/plansys2_executor/src/plansys2_executor/behavior_tree/apply_atend_effect_node.cpp index 16d6b00c..f83152e7 100644 --- a/plansys2_executor/src/plansys2_executor/behavior_tree/apply_atend_effect_node.cpp +++ b/plansys2_executor/src/plansys2_executor/behavior_tree/apply_atend_effect_node.cpp @@ -12,31 +12,27 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "plansys2_executor/behavior_tree/apply_atend_effect_node.hpp" + #include #include - -#include "plansys2_executor/behavior_tree/apply_atend_effect_node.hpp" +#include namespace plansys2 { - ApplyAtEndEffect::ApplyAtEndEffect( - const std::string & xml_tag_name, - const BT::NodeConfiguration & conf) + const std::string & xml_tag_name, const BT::NodeConfiguration & conf) : ActionNodeBase(xml_tag_name, conf) { action_map_ = config().blackboard->get>>( - "action_map"); + "action_map"); problem_client_ = - config().blackboard->get>( - "problem_client"); + config().blackboard->get>("problem_client"); } -BT::NodeStatus -ApplyAtEndEffect::tick() +BT::NodeStatus ApplyAtEndEffect::tick() { std::string action; getInput("action", action); diff --git a/plansys2_executor/src/plansys2_executor/behavior_tree/apply_atstart_effect_node.cpp b/plansys2_executor/src/plansys2_executor/behavior_tree/apply_atstart_effect_node.cpp index 7e6662c3..40a0358b 100644 --- a/plansys2_executor/src/plansys2_executor/behavior_tree/apply_atstart_effect_node.cpp +++ b/plansys2_executor/src/plansys2_executor/behavior_tree/apply_atstart_effect_node.cpp @@ -12,31 +12,27 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "plansys2_executor/behavior_tree/apply_atstart_effect_node.hpp" + #include #include - -#include "plansys2_executor/behavior_tree/apply_atstart_effect_node.hpp" +#include namespace plansys2 { - ApplyAtStartEffect::ApplyAtStartEffect( - const std::string & xml_tag_name, - const BT::NodeConfiguration & conf) + const std::string & xml_tag_name, const BT::NodeConfiguration & conf) : ActionNodeBase(xml_tag_name, conf) { action_map_ = config().blackboard->get>>( - "action_map"); + "action_map"); problem_client_ = - config().blackboard->get>( - "problem_client"); + config().blackboard->get>("problem_client"); } -BT::NodeStatus -ApplyAtStartEffect::tick() +BT::NodeStatus ApplyAtStartEffect::tick() { std::string action; getInput("action", action); diff --git a/plansys2_executor/src/plansys2_executor/behavior_tree/check_action_node.cpp b/plansys2_executor/src/plansys2_executor/behavior_tree/check_action_node.cpp index 8bf1d70a..bf31ef78 100644 --- a/plansys2_executor/src/plansys2_executor/behavior_tree/check_action_node.cpp +++ b/plansys2_executor/src/plansys2_executor/behavior_tree/check_action_node.cpp @@ -12,27 +12,23 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "plansys2_executor/behavior_tree/check_action_node.hpp" + #include #include - -#include "plansys2_executor/behavior_tree/check_action_node.hpp" +#include namespace plansys2 { - -CheckAction::CheckAction( - const std::string & xml_tag_name, - const BT::NodeConfiguration & conf) +CheckAction::CheckAction(const std::string & xml_tag_name, const BT::NodeConfiguration & conf) : ActionNodeBase(xml_tag_name, conf) { action_map_ = config().blackboard->get>>( - "action_map"); + "action_map"); } -BT::NodeStatus -CheckAction::tick() +BT::NodeStatus CheckAction::tick() { std::string action; getInput("action", action); @@ -41,11 +37,11 @@ CheckAction::tick() return BT::NodeStatus::RUNNING; // Not started yet } - if ((*action_map_)[action].action_executor != nullptr && + if ( + (*action_map_)[action].action_executor != nullptr && (*action_map_)[action].action_executor->is_finished() && (*action_map_)[action].at_start_effects_applied && - (*action_map_)[action].at_end_effects_applied) - { + (*action_map_)[action].at_end_effects_applied) { return BT::NodeStatus::SUCCESS; } else { return BT::NodeStatus::RUNNING; diff --git a/plansys2_executor/src/plansys2_executor/behavior_tree/check_atend_req_node.cpp b/plansys2_executor/src/plansys2_executor/behavior_tree/check_atend_req_node.cpp index d7c935ef..2acfdf4d 100644 --- a/plansys2_executor/src/plansys2_executor/behavior_tree/check_atend_req_node.cpp +++ b/plansys2_executor/src/plansys2_executor/behavior_tree/check_atend_req_node.cpp @@ -12,32 +12,27 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "plansys2_executor/behavior_tree/check_atend_req_node.hpp" + #include #include +#include #include -#include "plansys2_executor/behavior_tree/check_atend_req_node.hpp" - namespace plansys2 { - -CheckAtEndReq::CheckAtEndReq( - const std::string & xml_tag_name, - const BT::NodeConfiguration & conf) +CheckAtEndReq::CheckAtEndReq(const std::string & xml_tag_name, const BT::NodeConfiguration & conf) : ActionNodeBase(xml_tag_name, conf) { action_map_ = config().blackboard->get>>( - "action_map"); + "action_map"); problem_client_ = - config().blackboard->get>( - "problem_client"); + config().blackboard->get>("problem_client"); } -BT::NodeStatus -CheckAtEndReq::tick() +BT::NodeStatus CheckAtEndReq::tick() { std::string action; getInput("action", action); @@ -51,8 +46,9 @@ CheckAtEndReq::tick() RCLCPP_ERROR_STREAM( node->get_logger(), - "[" << action << "]" << (*action_map_)[action].execution_error_info << ": " << - parser::pddl::toString((*action_map_)[action].durative_action_info->at_end_requirements)); + "[" << action << "]" << (*action_map_)[action].execution_error_info << ": " + << parser::pddl::toString( + (*action_map_)[action].durative_action_info->at_end_requirements)); return BT::NodeStatus::FAILURE; } else { diff --git a/plansys2_executor/src/plansys2_executor/behavior_tree/check_overall_req_node.cpp b/plansys2_executor/src/plansys2_executor/behavior_tree/check_overall_req_node.cpp index 5b7cb223..d5c01c92 100644 --- a/plansys2_executor/src/plansys2_executor/behavior_tree/check_overall_req_node.cpp +++ b/plansys2_executor/src/plansys2_executor/behavior_tree/check_overall_req_node.cpp @@ -12,32 +12,28 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "plansys2_executor/behavior_tree/check_overall_req_node.hpp" + #include #include +#include #include -#include "plansys2_executor/behavior_tree/check_overall_req_node.hpp" - namespace plansys2 { - CheckOverAllReq::CheckOverAllReq( - const std::string & xml_tag_name, - const BT::NodeConfiguration & conf) + const std::string & xml_tag_name, const BT::NodeConfiguration & conf) : ActionNodeBase(xml_tag_name, conf) { action_map_ = config().blackboard->get>>( - "action_map"); + "action_map"); problem_client_ = - config().blackboard->get>( - "problem_client"); + config().blackboard->get>("problem_client"); } -BT::NodeStatus -CheckOverAllReq::tick() +BT::NodeStatus CheckOverAllReq::tick() { std::string action; getInput("action", action); @@ -51,8 +47,9 @@ CheckOverAllReq::tick() RCLCPP_ERROR_STREAM( node->get_logger(), - "[" << action << "]" << (*action_map_)[action].execution_error_info << ": " << - parser::pddl::toString((*action_map_)[action].durative_action_info->over_all_requirements)); + "[" << action << "]" << (*action_map_)[action].execution_error_info << ": " + << parser::pddl::toString( + (*action_map_)[action].durative_action_info->over_all_requirements)); return BT::NodeStatus::FAILURE; } else { diff --git a/plansys2_executor/src/plansys2_executor/behavior_tree/check_timeout_node.cpp b/plansys2_executor/src/plansys2_executor/behavior_tree/check_timeout_node.cpp index b49a6cca..93cfac66 100644 --- a/plansys2_executor/src/plansys2_executor/behavior_tree/check_timeout_node.cpp +++ b/plansys2_executor/src/plansys2_executor/behavior_tree/check_timeout_node.cpp @@ -12,34 +12,29 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "plansys2_executor/behavior_tree/check_timeout_node.hpp" + #include -#include +#include #include #include +#include #include -#include "plansys2_executor/behavior_tree/check_timeout_node.hpp" - namespace plansys2 { - -CheckTimeout::CheckTimeout( - const std::string & xml_tag_name, - const BT::NodeConfiguration & conf) +CheckTimeout::CheckTimeout(const std::string & xml_tag_name, const BT::NodeConfiguration & conf) : ActionNodeBase(xml_tag_name, conf) { action_map_ = config().blackboard->get>>( - "action_map"); + "action_map"); problem_client_ = - config().blackboard->get>( - "problem_client"); + config().blackboard->get>("problem_client"); } -BT::NodeStatus -CheckTimeout::tick() +BT::NodeStatus CheckTimeout::tick() { std::string action; getInput("action", action); @@ -55,11 +50,11 @@ CheckTimeout::tick() if (duration_overrun_percentage >= 0) { double max_duration = (1.0 + duration_overrun_percentage / 100.0) * duration; auto current_time = std::chrono::high_resolution_clock::now(); - auto elapsed_time = std::chrono::duration_cast( - current_time - start_); + auto elapsed_time = + std::chrono::duration_cast(current_time - start_); if (elapsed_time > std::chrono::duration(max_duration)) { - std::cerr << "Actual duration of " << action << " exceeds max duration (" << std::fixed << - std::setprecision(2) << max_duration << " secs)." << std::endl; + std::cerr << "Actual duration of " << action << " exceeds max duration (" << std::fixed + << std::setprecision(2) << max_duration << " secs)." << std::endl; return BT::NodeStatus::FAILURE; } } diff --git a/plansys2_executor/src/plansys2_executor/behavior_tree/execute_action_node.cpp b/plansys2_executor/src/plansys2_executor/behavior_tree/execute_action_node.cpp index 134d07f6..5eccc862 100644 --- a/plansys2_executor/src/plansys2_executor/behavior_tree/execute_action_node.cpp +++ b/plansys2_executor/src/plansys2_executor/behavior_tree/execute_action_node.cpp @@ -12,28 +12,24 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "plansys2_executor/behavior_tree/execute_action_node.hpp" + #include #include - -#include "plansys2_executor/behavior_tree/execute_action_node.hpp" +#include namespace plansys2 { - -ExecuteAction::ExecuteAction( - const std::string & xml_tag_name, - const BT::NodeConfiguration & conf) +ExecuteAction::ExecuteAction(const std::string & xml_tag_name, const BT::NodeConfiguration & conf) : ActionNodeBase(xml_tag_name, conf) { action_map_ = config().blackboard->get>>( - "action_map"); + "action_map"); node_ = config().blackboard->get("node"); } -void -ExecuteAction::halt() +void ExecuteAction::halt() { std::string action; getInput("action", action); @@ -46,8 +42,7 @@ ExecuteAction::halt() } } -BT::NodeStatus -ExecuteAction::tick() +BT::NodeStatus ExecuteAction::tick() { std::string action; getInput("action", action); @@ -65,8 +60,7 @@ ExecuteAction::tick() (*action_map_)[action].execution_error_info = "Error executing the action"; RCLCPP_ERROR_STREAM( - node_->get_logger(), - "[" << action << "]" << (*action_map_)[action].execution_error_info); + node_->get_logger(), "[" << action << "]" << (*action_map_)[action].execution_error_info); } return retval; diff --git a/plansys2_executor/src/plansys2_executor/behavior_tree/wait_action_node.cpp b/plansys2_executor/src/plansys2_executor/behavior_tree/wait_action_node.cpp index 7fd114b9..fa28181e 100644 --- a/plansys2_executor/src/plansys2_executor/behavior_tree/wait_action_node.cpp +++ b/plansys2_executor/src/plansys2_executor/behavior_tree/wait_action_node.cpp @@ -12,27 +12,23 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "plansys2_executor/behavior_tree/wait_action_node.hpp" + #include #include - -#include "plansys2_executor/behavior_tree/wait_action_node.hpp" +#include namespace plansys2 { - -WaitAction::WaitAction( - const std::string & xml_tag_name, - const BT::NodeConfiguration & conf) +WaitAction::WaitAction(const std::string & xml_tag_name, const BT::NodeConfiguration & conf) : ActionNodeBase(xml_tag_name, conf) { action_map_ = config().blackboard->get>>( - "action_map"); + "action_map"); } -BT::NodeStatus -WaitAction::tick() +BT::NodeStatus WaitAction::tick() { std::string action; getInput("action", action); @@ -41,11 +37,11 @@ WaitAction::tick() return BT::NodeStatus::RUNNING; // Not started yet } - if ((*action_map_)[action].action_executor != nullptr && + if ( + (*action_map_)[action].action_executor != nullptr && (*action_map_)[action].action_executor->is_finished() && (*action_map_)[action].at_start_effects_applied && - (*action_map_)[action].at_end_effects_applied) - { + (*action_map_)[action].at_end_effects_applied) { return BT::NodeStatus::SUCCESS; } else { return BT::NodeStatus::RUNNING; diff --git a/plansys2_executor/src/plansys2_executor/behavior_tree/wait_atstart_req_node.cpp b/plansys2_executor/src/plansys2_executor/behavior_tree/wait_atstart_req_node.cpp index 370e2caf..c592ad81 100644 --- a/plansys2_executor/src/plansys2_executor/behavior_tree/wait_atstart_req_node.cpp +++ b/plansys2_executor/src/plansys2_executor/behavior_tree/wait_atstart_req_node.cpp @@ -12,32 +12,27 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "plansys2_executor/behavior_tree/wait_atstart_req_node.hpp" + #include #include +#include #include -#include "plansys2_executor/behavior_tree/wait_atstart_req_node.hpp" - namespace plansys2 { - -WaitAtStartReq::WaitAtStartReq( - const std::string & xml_tag_name, - const BT::NodeConfiguration & conf) +WaitAtStartReq::WaitAtStartReq(const std::string & xml_tag_name, const BT::NodeConfiguration & conf) : ActionNodeBase(xml_tag_name, conf) { action_map_ = config().blackboard->get>>( - "action_map"); + "action_map"); problem_client_ = - config().blackboard->get>( - "problem_client"); + config().blackboard->get>("problem_client"); } -BT::NodeStatus -WaitAtStartReq::tick() +BT::NodeStatus WaitAtStartReq::tick() { std::string action; getInput("action", action); @@ -53,8 +48,9 @@ WaitAtStartReq::tick() RCLCPP_ERROR_STREAM( node->get_logger(), - "[" << action << "]" << (*action_map_)[action].execution_error_info << ": " << - parser::pddl::toString((*action_map_)[action].durative_action_info->at_start_requirements)); + "[" << action << "]" << (*action_map_)[action].execution_error_info << ": " + << parser::pddl::toString( + (*action_map_)[action].durative_action_info->at_start_requirements)); return BT::NodeStatus::RUNNING; } @@ -65,8 +61,9 @@ WaitAtStartReq::tick() RCLCPP_ERROR_STREAM( node->get_logger(), - "[" << action << "]" << (*action_map_)[action].execution_error_info << ": " << - parser::pddl::toString((*action_map_)[action].durative_action_info->over_all_requirements)); + "[" << action << "]" << (*action_map_)[action].execution_error_info << ": " + << parser::pddl::toString( + (*action_map_)[action].durative_action_info->over_all_requirements)); return BT::NodeStatus::RUNNING; } diff --git a/plansys2_executor/src/plansys2_executor/bt_builder_plugins/simple_bt_builder.cpp b/plansys2_executor/src/plansys2_executor/bt_builder_plugins/simple_bt_builder.cpp index 1bae86e6..47a45a32 100644 --- a/plansys2_executor/src/plansys2_executor/bt_builder_plugins/simple_bt_builder.cpp +++ b/plansys2_executor/src/plansys2_executor/bt_builder_plugins/simple_bt_builder.cpp @@ -12,37 +12,32 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include -#include +#include "plansys2_executor/bt_builder_plugins/simple_bt_builder.hpp" + #include #include -#include #include +#include +#include +#include +#include #include +#include -#include "plansys2_executor/bt_builder_plugins/simple_bt_builder.hpp" - -#include "plansys2_problem_expert/Utils.hpp" #include "plansys2_pddl_parser/Utils.h" - +#include "plansys2_problem_expert/Utils.hpp" #include "rclcpp/rclcpp.hpp" namespace plansys2 { - SimpleBTBuilder::SimpleBTBuilder() { domain_client_ = std::make_shared(); problem_client_ = std::make_shared(); } -void -SimpleBTBuilder::initialize( - const std::string & bt_action_1, - const std::string & bt_action_2, - int precision) +void SimpleBTBuilder::initialize( + const std::string & bt_action_1, const std::string & bt_action_2, int precision) { if (bt_action_1 != "") { bt_action_ = bt_action_1; @@ -62,20 +57,16 @@ WAIT_PREV_ACTIONS } } -bool -SimpleBTBuilder::is_action_executable( - const ActionStamped & action, - std::vector & predicates, +bool SimpleBTBuilder::is_action_executable( + const ActionStamped & action, std::vector & predicates, std::vector & functions) const { return check(action.action->at_start_requirements, predicates, functions) && check(action.action->over_all_requirements, predicates, functions); } -GraphNode::Ptr -SimpleBTBuilder::get_node_satisfy( - const plansys2_msgs::msg::Tree & requirement, - const GraphNode::Ptr & node, +GraphNode::Ptr SimpleBTBuilder::get_node_satisfy( + const plansys2_msgs::msg::Tree & requirement, const GraphNode::Ptr & node, const GraphNode::Ptr & current) { if (node == current) { @@ -114,10 +105,8 @@ SimpleBTBuilder::get_node_satisfy( return ret; } -void -SimpleBTBuilder::get_node_contradict( - const GraphNode::Ptr & node, - const GraphNode::Ptr & current, +void SimpleBTBuilder::get_node_contradict( + const GraphNode::Ptr & node, const GraphNode::Ptr & current, std::list & contradictions) { if (node == current) { @@ -145,12 +134,9 @@ SimpleBTBuilder::get_node_contradict( } } -bool -SimpleBTBuilder::is_parallelizable( - const plansys2::ActionStamped & action, - const std::vector & predicates, - const std::vector & functions, - const std::list & nodes) const +bool SimpleBTBuilder::is_parallelizable( + const plansys2::ActionStamped & action, const std::vector & predicates, + const std::vector & functions, const std::list & nodes) const { // Apply the "at start" effects of the new action. auto preds = predicates; @@ -180,10 +166,8 @@ SimpleBTBuilder::is_parallelizable( return true; } -GraphNode::Ptr -SimpleBTBuilder::get_node_satisfy( - const plansys2_msgs::msg::Tree & requirement, - const Graph::Ptr & graph, +GraphNode::Ptr SimpleBTBuilder::get_node_satisfy( + const plansys2_msgs::msg::Tree & requirement, const Graph::Ptr & graph, const GraphNode::Ptr & current) { GraphNode::Ptr ret; @@ -197,10 +181,8 @@ SimpleBTBuilder::get_node_satisfy( return ret; } -std::list -SimpleBTBuilder::get_node_contradict( - const Graph::Ptr & graph, - const GraphNode::Ptr & current) +std::list SimpleBTBuilder::get_node_contradict( + const Graph::Ptr & graph, const GraphNode::Ptr & current) { std::list ret; @@ -211,11 +193,9 @@ SimpleBTBuilder::get_node_contradict( return ret; } -std::list -SimpleBTBuilder::get_roots( +std::list SimpleBTBuilder::get_roots( std::vector & action_sequence, - std::vector & predicates, - std::vector & functions, + std::vector & predicates, std::vector & functions, int & node_counter) { std::list ret; @@ -223,9 +203,9 @@ SimpleBTBuilder::get_roots( auto it = action_sequence.begin(); while (it != action_sequence.end()) { const auto & action = *it; - if (is_action_executable(action, predicates, functions) && - is_parallelizable(action, predicates, functions, ret)) - { + if ( + is_action_executable(action, predicates, functions) && + is_parallelizable(action, predicates, functions, ret)) { auto new_root = GraphNode::make_shared(); new_root->action = action; new_root->node_num = node_counter++; @@ -243,11 +223,9 @@ SimpleBTBuilder::get_roots( return ret; } -void -SimpleBTBuilder::remove_existing_requirements( +void SimpleBTBuilder::remove_existing_requirements( std::vector & requirements, - std::vector & predicates, - std::vector & functions) const + std::vector & predicates, std::vector & functions) const { auto it = requirements.begin(); while (it != requirements.end()) { @@ -259,8 +237,7 @@ SimpleBTBuilder::remove_existing_requirements( } } -void -SimpleBTBuilder::prune_backwards(GraphNode::Ptr new_node, GraphNode::Ptr node_satisfy) +void SimpleBTBuilder::prune_backwards(GraphNode::Ptr new_node, GraphNode::Ptr node_satisfy) { // Repeat prune to the roots for (auto & in : node_satisfy->in_arcs) { @@ -278,8 +255,7 @@ SimpleBTBuilder::prune_backwards(GraphNode::Ptr new_node, GraphNode::Ptr node_sa } } -void -SimpleBTBuilder::prune_forward(GraphNode::Ptr current, std::list & used_nodes) +void SimpleBTBuilder::prune_forward(GraphNode::Ptr current, std::list & used_nodes) { auto it = current->out_arcs.begin(); while (it != current->out_arcs.end()) { @@ -294,12 +270,9 @@ SimpleBTBuilder::prune_forward(GraphNode::Ptr current, std::list } } -void -SimpleBTBuilder::get_state( - const GraphNode::Ptr & node, - std::list & used_nodes, - std::vector & predicates, - std::vector & functions) const +void SimpleBTBuilder::get_state( + const GraphNode::Ptr & node, std::list & used_nodes, + std::vector & predicates, std::vector & functions) const { // Traverse graph to the root for (auto & in : node->in_arcs) { @@ -312,8 +285,7 @@ SimpleBTBuilder::get_state( } } -Graph::Ptr -SimpleBTBuilder::get_graph(const plansys2_msgs::msg::Plan & current_plan) +Graph::Ptr SimpleBTBuilder::get_graph(const plansys2_msgs::msg::Plan & current_plan) { int node_counter = 0; int level_counter = 0; @@ -353,17 +325,15 @@ SimpleBTBuilder::get_graph(const plansys2_msgs::msg::Plan & current_plan) std::vector requirements; requirements.insert( - std::end(requirements), std::begin(at_start_requirements), - std::end(at_start_requirements)); + std::end(requirements), std::begin(at_start_requirements), std::end(at_start_requirements)); requirements.insert( - std::end(requirements), std::begin(over_all_requirements), - std::end(over_all_requirements)); + std::end(requirements), std::begin(over_all_requirements), std::end(over_all_requirements)); requirements.insert( - std::end(requirements), std::begin(at_end_requirements), - std::end(at_end_requirements)); + std::end(requirements), std::begin(at_end_requirements), std::end(at_end_requirements)); // Look for satisfying nodes - // A satisfying node is a node with an effect that satisfies a requirement of the new node + // A satisfying node is a node with an effect that satisfies a requirement + // of the new node auto it = requirements.begin(); while (it != requirements.end()) { auto parent = get_node_satisfy(*it, graph, new_node); @@ -371,14 +341,14 @@ SimpleBTBuilder::get_graph(const plansys2_msgs::msg::Plan & current_plan) prune_backwards(new_node, parent); // Create the connections to the parent node - if (std::find(new_node->in_arcs.begin(), new_node->in_arcs.end(), parent) == - new_node->in_arcs.end()) - { + if ( + std::find(new_node->in_arcs.begin(), new_node->in_arcs.end(), parent) == + new_node->in_arcs.end()) { new_node->in_arcs.push_back(parent); } - if (std::find(parent->out_arcs.begin(), parent->out_arcs.end(), new_node) == - parent->out_arcs.end()) - { + if ( + std::find(parent->out_arcs.begin(), parent->out_arcs.end(), new_node) == + parent->out_arcs.end()) { parent->out_arcs.push_back(new_node); } @@ -389,20 +359,21 @@ SimpleBTBuilder::get_graph(const plansys2_msgs::msg::Plan & current_plan) } // Look for contradicting parallel actions - // A1 and A2 cannot run in parallel if the effects of A1 contradict the requirements of A2 + // A1 and A2 cannot run in parallel if the effects of A1 contradict the + // requirements of A2 auto contradictions = get_node_contradict(graph, new_node); for (const auto parent : contradictions) { prune_backwards(new_node, parent); // Create the connections to the parent node - if (std::find(new_node->in_arcs.begin(), new_node->in_arcs.end(), parent) == - new_node->in_arcs.end()) - { + if ( + std::find(new_node->in_arcs.begin(), new_node->in_arcs.end(), parent) == + new_node->in_arcs.end()) { new_node->in_arcs.push_back(parent); } - if (std::find(parent->out_arcs.begin(), parent->out_arcs.end(), new_node) == - parent->out_arcs.end()) - { + if ( + std::find(parent->out_arcs.begin(), parent->out_arcs.end(), new_node) == + parent->out_arcs.end()) { parent->out_arcs.push_back(new_node); } } @@ -420,8 +391,8 @@ SimpleBTBuilder::get_graph(const plansys2_msgs::msg::Plan & current_plan) // These should be satisfied by the initial state. remove_existing_requirements(requirements, predicates, functions); for (const auto & req : requirements) { - std::cerr << "[ERROR] requirement not met: [" << - parser::pddl::toString(req) << "]" << std::endl; + std::cerr << "[ERROR] requirement not met: [" << parser::pddl::toString(req) << "]" + << std::endl; } assert(requirements.empty()); @@ -431,8 +402,7 @@ SimpleBTBuilder::get_graph(const plansys2_msgs::msg::Plan & current_plan) return graph; } -std::string -SimpleBTBuilder::get_tree(const plansys2_msgs::msg::Plan & current_plan) +std::string SimpleBTBuilder::get_tree(const plansys2_msgs::msg::Plan & current_plan) { graph_ = get_graph(current_plan); @@ -444,20 +414,18 @@ SimpleBTBuilder::get_tree(const plansys2_msgs::msg::Plan & current_plan) std::list used_nodes; if (graph_->roots.size() > 1) { - bt_ = std::string("\n") + - t(1) + "\n" + - t(2) + "roots.size()) + - "\" failure_threshold=\"1\">\n"; + bt_ = std::string("\n") + t(1) + + "\n" + t(2) + "roots.size()) + "\" failure_threshold=\"1\">\n"; for (const auto & node : graph_->roots) { bt_ = bt_ + get_flow_tree(node, used_nodes, 3); } - bt_ = bt_ + t(2) + "\n" + - t(1) + "\n\n"; + bt_ = bt_ + t(2) + "\n" + t(1) + "\n\n"; } else { - bt_ = std::string("\n") + - t(1) + "\n"; + bt_ = std::string("\n") + t(1) + + "\n"; bt_ = bt_ + get_flow_tree(*graph_->roots.begin(), used_nodes, 2); @@ -467,10 +435,8 @@ SimpleBTBuilder::get_tree(const plansys2_msgs::msg::Plan & current_plan) return bt_; } -std::string -SimpleBTBuilder::get_dotgraph( - std::shared_ptr> action_map, - bool enable_legend, +std::string SimpleBTBuilder::get_dotgraph( + std::shared_ptr> action_map, bool enable_legend, bool enable_print_graph) { if (enable_print_graph) { @@ -567,18 +533,14 @@ SimpleBTBuilder::get_dotgraph( return ss.str(); } -std::string -SimpleBTBuilder::get_flow_tree( - GraphNode::Ptr node, - std::list & used_nodes, - int level) +std::string SimpleBTBuilder::get_flow_tree( + GraphNode::Ptr node, std::list & used_nodes, int level) { std::string ret; int l = level; const std::string action_id = "(" + parser::pddl::nameActionsToString(node->action.action) + - "):" + - std::to_string(static_cast(node->action.time * 1000)); + "):" + std::to_string(static_cast(node->action.time * 1000)); if (std::find(used_nodes.begin(), used_nodes.end(), action_id) != used_nodes.end()) { return t(l) + "\n"; @@ -601,9 +563,8 @@ SimpleBTBuilder::get_flow_tree( ret = ret + t(l) + "\n"; ret = ret + execution_block(node, l + 1); - ret = ret + t(l + 1) + - "out_arcs.size()) + - "\" failure_threshold=\"1\">\n"; + ret = ret + t(l + 1) + "out_arcs.size()) + "\" failure_threshold=\"1\">\n"; for (const auto & child_node : node->out_arcs) { ret = ret + get_flow_tree(child_node, used_nodes, l + 2); @@ -616,28 +577,24 @@ SimpleBTBuilder::get_flow_tree( return ret; } -void -SimpleBTBuilder::get_flow_dotgraph( - GraphNode::Ptr node, - std::set & edges) +void SimpleBTBuilder::get_flow_dotgraph(GraphNode::Ptr node, std::set & edges) { for (const auto & arc : node->out_arcs) { - std::string edge = std::to_string(node->node_num) + "->" + std::to_string(arc->node_num) + - ";\n"; + std::string edge = + std::to_string(node->node_num) + "->" + std::to_string(arc->node_num) + ";\n"; edges.insert(edge); get_flow_dotgraph(arc, edges); } } -std::string -SimpleBTBuilder::get_node_dotgraph( - GraphNode::Ptr node, std::shared_ptr> action_map, int level) +std::string SimpleBTBuilder::get_node_dotgraph( + GraphNode::Ptr node, std::shared_ptr> action_map, + int level) { std::stringstream ss; ss << t(level); - ss << node->node_num << " [label=\"" << parser::pddl::nameActionsToString(node->action.action) << - "\""; + ss << node->node_num << " [label=\"" << parser::pddl::nameActionsToString(node->action.action) + << "\""; ss << "labeljust=c,style=filled"; auto status = get_action_status(node->action, action_map); @@ -668,8 +625,8 @@ ActionExecutor::Status SimpleBTBuilder::get_action_status( ActionStamped action_stamped, std::shared_ptr> action_map) { - auto index = "(" + parser::pddl::nameActionsToString(action_stamped.action) + "):" + - std::to_string(static_cast(action_stamped.time * 1000)); + auto index = "(" + parser::pddl::nameActionsToString(action_stamped.action) + + "):" + std::to_string(static_cast(action_stamped.time * 1000)); if ((*action_map)[index].action_executor) { return (*action_map)[index].action_executor->get_internal_status(); } else { @@ -678,8 +635,7 @@ ActionExecutor::Status SimpleBTBuilder::get_action_status( } void SimpleBTBuilder::addDotGraphLegend( - std::stringstream & ss, int tab_level, int level_counter, - int node_counter) + std::stringstream & ss, int tab_level, int level_counter, int node_counter) { int legend_counter = level_counter; int legend_node_counter = node_counter; @@ -703,17 +659,21 @@ void SimpleBTBuilder::addDotGraphLegend( ss << t(tab_level); ss << "labeljust = l;\n"; ss << t(tab_level); - ss << legend_node_counter++ << - " [label=\n\"Finished action\n\",labeljust=c,style=filled,color=green4,fillcolor=seagreen2];\n"; + ss << legend_node_counter++ + << " [label=\n\"Finished " + "action\n\",labeljust=c,style=filled,color=green4,fillcolor=seagreen2];" + "\n"; ss << t(tab_level); - ss << legend_node_counter++ << - " [label=\n\"Failed action\n\",labeljust=c,style=filled,color=red,fillcolor=pink];\n"; + ss << legend_node_counter++ + << " [label=\n\"Failed " + "action\n\",labeljust=c,style=filled,color=red,fillcolor=pink];\n"; ss << t(tab_level); - ss << legend_node_counter++ << - " [label=\n\"Current action\n\",labeljust=c,style=filled,color=blue,fillcolor=skyblue];\n"; + ss << legend_node_counter++ + << " [label=\n\"Current " + "action\n\",labeljust=c,style=filled,color=blue,fillcolor=skyblue];\n"; ss << t(tab_level); - ss << legend_node_counter++ << " [label=\n\"Future action\n\",labeljust=c,style=filled," << - "color=yellow3,fillcolor=lightgoldenrod1];\n"; + ss << legend_node_counter++ << " [label=\n\"Future action\n\",labeljust=c,style=filled," + << "color=yellow3,fillcolor=lightgoldenrod1];\n"; tab_level--; ss << t(tab_level); ss << "}\n"; @@ -732,8 +692,7 @@ void SimpleBTBuilder::addDotGraphLegend( ss << "}\n"; } -std::string -SimpleBTBuilder::t(int level) +std::string SimpleBTBuilder::t(int level) { std::string ret; for (int i = 0; i < level; i++) { @@ -750,20 +709,19 @@ void replace(std::string & str, const std::string & from, const std::string & to } } -std::string -SimpleBTBuilder::execution_block(const GraphNode::Ptr & node, int l) +std::string SimpleBTBuilder::execution_block(const GraphNode::Ptr & node, int l) { const auto & action = node->action; std::string ret; std::string ret_aux = bt_action_; - const std::string action_id = "(" + parser::pddl::nameActionsToString(action.action) + "):" + - std::to_string(static_cast(action.time * 1000)); + const std::string action_id = "(" + parser::pddl::nameActionsToString(action.action) + + "):" + std::to_string(static_cast(action.time * 1000)); std::string wait_actions; for (const auto & previous_node : node->in_arcs) { - const std::string parent_action_id = "(" + - parser::pddl::nameActionsToString(previous_node->action.action) + "):" + - std::to_string(static_cast( previous_node->action.time * 1000)); + const std::string parent_action_id = + "(" + parser::pddl::nameActionsToString(previous_node->action.action) + + "):" + std::to_string(static_cast(previous_node->action.time * 1000)); wait_actions = wait_actions + t(1) + ""; if (previous_node != *node->in_arcs.rbegin()) { @@ -784,8 +742,7 @@ SimpleBTBuilder::execution_block(const GraphNode::Ptr & node, int l) return ret; } -std::vector -SimpleBTBuilder::get_plan_actions(const plansys2_msgs::msg::Plan & plan) +std::vector SimpleBTBuilder::get_plan_actions(const plansys2_msgs::msg::Plan & plan) { std::vector ret; @@ -794,8 +751,7 @@ SimpleBTBuilder::get_plan_actions(const plansys2_msgs::msg::Plan & plan) action_stamped.time = item.time; action_stamped.duration = item.duration; - action_stamped.action = - domain_client_->getDurativeAction( + action_stamped.action = domain_client_->getDurativeAction( get_action_name(item.action), get_action_params(item.action)); ret.push_back(action_stamped); @@ -804,10 +760,8 @@ SimpleBTBuilder::get_plan_actions(const plansys2_msgs::msg::Plan & plan) return ret; } -void -SimpleBTBuilder::print_node( - const plansys2::GraphNode::Ptr & node, - int level, +void SimpleBTBuilder::print_node( + const plansys2::GraphNode::Ptr & node, int level, std::set & used_nodes) const { std::cerr << std::string(level, '\t') << "[" << node->action.time << "] "; @@ -823,8 +777,7 @@ SimpleBTBuilder::print_node( } } -void -SimpleBTBuilder::print_graph(const plansys2::Graph::Ptr & graph) const +void SimpleBTBuilder::print_graph(const plansys2::Graph::Ptr & graph) const { std::set used_nodes; for (const auto & root : graph->roots) { @@ -832,13 +785,11 @@ SimpleBTBuilder::print_graph(const plansys2::Graph::Ptr & graph) const } } -void -SimpleBTBuilder::print_node_csv(const plansys2::GraphNode::Ptr & node, uint32_t root_num) const +void SimpleBTBuilder::print_node_csv(const plansys2::GraphNode::Ptr & node, uint32_t root_num) const { - std::string out_str = std::to_string(root_num) + ", " + - std::to_string(node->node_num) + ", " + - std::to_string(node->level_num) + ", " + - parser::pddl::nameActionsToString(node->action.action); + std::string out_str = std::to_string(root_num) + ", " + std::to_string(node->node_num) + ", " + + std::to_string(node->level_num) + ", " + + parser::pddl::nameActionsToString(node->action.action); for (const auto & arc : node->out_arcs) { out_str = out_str + ", " + parser::pddl::nameActionsToString(arc->action.action); } @@ -848,8 +799,7 @@ SimpleBTBuilder::print_node_csv(const plansys2::GraphNode::Ptr & node, uint32_t } } -void -SimpleBTBuilder::print_graph_csv(const plansys2::Graph::Ptr & graph) const +void SimpleBTBuilder::print_graph_csv(const plansys2::Graph::Ptr & graph) const { uint32_t root_num = 0; for (const auto & root : graph->roots) { @@ -858,16 +808,13 @@ SimpleBTBuilder::print_graph_csv(const plansys2::Graph::Ptr & graph) const } } -void -SimpleBTBuilder::get_node_tabular( - const plansys2::GraphNode::Ptr & node, - uint32_t root_num, +void SimpleBTBuilder::get_node_tabular( + const plansys2::GraphNode::Ptr & node, uint32_t root_num, std::vector> & graph) const { - graph.push_back( - std::make_tuple( - root_num, node->node_num, node->level_num, - parser::pddl::nameActionsToString(node->action.action))); + graph.push_back(std::make_tuple( + root_num, node->node_num, node->level_num, + parser::pddl::nameActionsToString(node->action.action))); for (const auto & out : node->out_arcs) { get_node_tabular(out, root_num, graph); } diff --git a/plansys2_executor/src/plansys2_executor/bt_builder_plugins/stn_bt_builder.cpp b/plansys2_executor/src/plansys2_executor/bt_builder_plugins/stn_bt_builder.cpp index 34d1bd2f..2194b465 100644 --- a/plansys2_executor/src/plansys2_executor/bt_builder_plugins/stn_bt_builder.cpp +++ b/plansys2_executor/src/plansys2_executor/bt_builder_plugins/stn_bt_builder.cpp @@ -28,18 +28,14 @@ namespace plansys2 { - STNBTBuilder::STNBTBuilder() { domain_client_ = std::make_shared(); problem_client_ = std::make_shared(); } -void -STNBTBuilder::initialize( - const std::string & bt_action_1, - const std::string & bt_action_2, - int precision) +void STNBTBuilder::initialize( + const std::string & bt_action_1, const std::string & bt_action_2, int precision) { if (bt_action_1 != "") { bt_start_action_ = bt_action_1; @@ -72,18 +68,15 @@ CHECK_PREV_ACTIONS action_time_precision_ = precision; } -std::string -STNBTBuilder::get_tree(const plansys2_msgs::msg::Plan & plan) +std::string STNBTBuilder::get_tree(const plansys2_msgs::msg::Plan & plan) { stn_ = build_stn(plan); auto bt = build_bt(stn_); return bt; } -std::string -STNBTBuilder::get_dotgraph( - std::shared_ptr> action_map, - bool enable_legend, +std::string STNBTBuilder::get_dotgraph( + std::shared_ptr> action_map, bool enable_legend, bool enable_print_graph) { if (enable_print_graph) { @@ -134,8 +127,7 @@ STNBTBuilder::get_dotgraph( return ss.str(); } -Graph::Ptr -STNBTBuilder::build_stn(const plansys2_msgs::msg::Plan & plan) const +Graph::Ptr STNBTBuilder::build_stn(const plansys2_msgs::msg::Plan & plan) const { auto stn = init_graph(plan); auto happenings = get_happenings(plan); @@ -177,22 +169,20 @@ STNBTBuilder::build_stn(const plansys2_msgs::msg::Plan & plan) const return stn; } -std::string -STNBTBuilder::build_bt(const Graph::Ptr stn) const +std::string STNBTBuilder::build_bt(const Graph::Ptr stn) const { std::set used; const auto & root = stn->nodes.front(); auto bt = std::string("\n") + t(1) + - "\n"; + "\n"; bt = bt + get_flow(root, root, used, 1); bt = bt + t(1) + "\n\n"; return bt; } -Graph::Ptr -STNBTBuilder::init_graph(const plansys2_msgs::msg::Plan & plan) const +Graph::Ptr STNBTBuilder::init_graph(const plansys2_msgs::msg::Plan & plan) const { auto graph = Graph::make_shared(); auto action_sequence = get_plan_actions(plan); @@ -238,8 +228,8 @@ STNBTBuilder::init_graph(const plansys2_msgs::msg::Plan & plan) const return graph; } -std::vector -STNBTBuilder::get_plan_actions(const plansys2_msgs::msg::Plan & plan) const +std::vector STNBTBuilder::get_plan_actions( + const plansys2_msgs::msg::Plan & plan) const { std::vector ret; @@ -249,8 +239,7 @@ STNBTBuilder::get_plan_actions(const plansys2_msgs::msg::Plan & plan) const action_stamped.expression = item.action; action_stamped.duration = item.duration; action_stamped.type = ActionType::DURATIVE; - action_stamped.action = - domain_client_->getDurativeAction( + action_stamped.action = domain_client_->getDurativeAction( get_action_name(item.action), get_action_params(item.action)); ret.push_back(action_stamped); @@ -259,8 +248,7 @@ STNBTBuilder::get_plan_actions(const plansys2_msgs::msg::Plan & plan) const return ret; } -std::set -STNBTBuilder::get_happenings(const plansys2_msgs::msg::Plan & plan) const +std::set STNBTBuilder::get_happenings(const plansys2_msgs::msg::Plan & plan) const { std::set happenings; happenings.insert(-1); @@ -274,8 +262,8 @@ STNBTBuilder::get_happenings(const plansys2_msgs::msg::Plan & plan) const return happenings; } -std::set::iterator -STNBTBuilder::get_happening(int time, const std::set & happenings) const +std::set::iterator STNBTBuilder::get_happening( + int time, const std::set & happenings) const { // This function returns an iterator pointing to either // 1. the first element that is equal to the key or @@ -298,8 +286,7 @@ STNBTBuilder::get_happening(int time, const std::set & happenings) const return it; } -std::set::iterator -STNBTBuilder::get_previous(int time, const std::set & happenings) const +std::set::iterator STNBTBuilder::get_previous(int time, const std::set & happenings) const { auto it = get_happening(time, happenings); @@ -312,8 +299,8 @@ STNBTBuilder::get_previous(int time, const std::set & happenings) const return happenings.end(); } -std::multimap -STNBTBuilder::get_simple_plan(const plansys2_msgs::msg::Plan & plan) const +std::multimap STNBTBuilder::get_simple_plan( + const plansys2_msgs::msg::Plan & plan) const { std::multimap simple_plan; auto action_sequence = get_plan_actions(plan); @@ -358,19 +345,15 @@ STNBTBuilder::get_simple_plan(const plansys2_msgs::msg::Plan & plan) const // Find the start action auto it = simple_plan.equal_range(time); - auto start_action = std::find_if( - it.first, it.second, - [&](std::pair a) { - return (a.second.expression == action.expression) && (a.second.type == ActionType::START); - }); + auto start_action = std::find_if(it.first, it.second, [&](std::pair a) { + return (a.second.expression == action.expression) && (a.second.type == ActionType::START); + }); // Find the end action it = simple_plan.equal_range(time + duration); - auto end_action = std::find_if( - it.first, it.second, - [&](std::pair a) { - return (a.second.expression == action.expression) && (a.second.type == ActionType::END); - }); + auto end_action = std::find_if(it.first, it.second, [&](std::pair a) { + return (a.second.expression == action.expression) && (a.second.type == ActionType::END); + }); // Compute the overall actions int prev = time; @@ -396,10 +379,8 @@ STNBTBuilder::get_simple_plan(const plansys2_msgs::msg::Plan & plan) const return simple_plan; } -std::map -STNBTBuilder::get_states( - const std::set & happenings, - const std::multimap & plan) const +std::map STNBTBuilder::get_states( + const std::set & happenings, const std::multimap & plan) const { std::map states; @@ -423,8 +404,7 @@ STNBTBuilder::get_states( return states; } -plansys2_msgs::msg::Tree -STNBTBuilder::from_state( +plansys2_msgs::msg::Tree STNBTBuilder::from_state( const std::vector & preds, const std::vector & funcs) const { @@ -452,18 +432,15 @@ STNBTBuilder::from_state( return tree; } -std::vector -STNBTBuilder::get_nodes( - const ActionStamped & action, - const Graph::Ptr graph) const +std::vector STNBTBuilder::get_nodes( + const ActionStamped & action, const Graph::Ptr graph) const { std::vector ret; if (action.type == ActionType::INIT) { - auto it = std::find_if( - graph->nodes.begin(), graph->nodes.end(), [&](GraphNode::Ptr node) { - return node->action.type == ActionType::INIT; - }); + auto it = std::find_if(graph->nodes.begin(), graph->nodes.end(), [&](GraphNode::Ptr node) { + return node->action.type == ActionType::INIT; + }); if (it != graph->nodes.end()) { ret.push_back(*it); } else { @@ -473,10 +450,9 @@ STNBTBuilder::get_nodes( } if (action.type == ActionType::GOAL) { - auto it = std::find_if( - graph->nodes.begin(), graph->nodes.end(), [&](GraphNode::Ptr node) { - return node->action.type == ActionType::GOAL; - }); + auto it = std::find_if(graph->nodes.begin(), graph->nodes.end(), [&](GraphNode::Ptr node) { + return node->action.type == ActionType::GOAL; + }); if (it != graph->nodes.end()) { ret.push_back(*it); } else { @@ -491,10 +467,9 @@ STNBTBuilder::get_nodes( std::bind(&STNBTBuilder::is_match, this, std::placeholders::_1, action)); if (action.type == ActionType::START || action.type == ActionType::OVERALL) { - auto it = std::find_if( - matches.begin(), matches.end(), [&](GraphNode::Ptr node) { - return node->action.type == ActionType::START; - }); + auto it = std::find_if(matches.begin(), matches.end(), [&](GraphNode::Ptr node) { + return node->action.type == ActionType::START; + }); if (it != matches.end()) { ret.push_back(*it); } else { @@ -503,10 +478,9 @@ STNBTBuilder::get_nodes( } if (action.type == ActionType::END || action.type == ActionType::OVERALL) { - auto it = std::find_if( - matches.begin(), matches.end(), [&](GraphNode::Ptr node) { - return node->action.type == ActionType::END; - }); + auto it = std::find_if(matches.begin(), matches.end(), [&](GraphNode::Ptr node) { + return node->action.type == ActionType::END; + }); if (it != matches.end()) { ret.push_back(*it); } else { @@ -521,22 +495,16 @@ STNBTBuilder::get_nodes( return ret; } -bool -STNBTBuilder::is_match( - const GraphNode::Ptr node, - const ActionStamped & action) const +bool STNBTBuilder::is_match(const GraphNode::Ptr node, const ActionStamped & action) const { auto t_1 = to_int_time(node->action.time, action_time_precision_ + 1); auto t_2 = to_int_time(action.time, action_time_precision_ + 1); return (t_1 == t_2) && (node->action.expression == action.expression); } -std::vector> -STNBTBuilder::get_parents( - const std::pair & action, - const std::multimap & plan, - const std::set & happenings, - const std::map & states) const +std::vector> STNBTBuilder::get_parents( + const std::pair & action, const std::multimap & plan, + const std::set & happenings, const std::map & states) const { auto parents = get_satisfy(action, plan, happenings, states); auto threats = get_threat(action, plan, happenings, states); @@ -545,12 +513,9 @@ STNBTBuilder::get_parents( return parents; } -std::vector> -STNBTBuilder::get_satisfy( - const std::pair & action, - const std::multimap & plan, - const std::set & happenings, - const std::map & states) const +std::vector> STNBTBuilder::get_satisfy( + const std::pair & action, const std::multimap & plan, + const std::set & happenings, const std::map & states) const { std::vector> ret; @@ -615,12 +580,9 @@ STNBTBuilder::get_satisfy( return ret; } -std::vector> -STNBTBuilder::get_threat( - const std::pair & action, - const std::multimap & plan, - const std::set & happenings, - const std::map & states) const +std::vector> STNBTBuilder::get_threat( + const std::pair & action, const std::multimap & plan, + const std::set & happenings, const std::map & states) const { std::vector> ret; @@ -679,9 +641,9 @@ STNBTBuilder::get_threat( apply(E_a, X_hat.predicates, X_hat.functions); // Check if the input action threatens action k - if (action.second.type != ActionType::OVERALL && - !check(R_k, X_hat.predicates, X_hat.functions)) - { + if ( + action.second.type != ActionType::OVERALL && + !check(R_k, X_hat.predicates, X_hat.functions)) { if (t_2 != t_in) { ret.push_back(*iter); } else { @@ -696,9 +658,9 @@ STNBTBuilder::get_threat( apply(E_k, X_bar.predicates, X_bar.functions); // Check if action k threatens the input action - if (iter->second.type != ActionType::OVERALL && - !check(R_a, X_bar.predicates, X_bar.functions)) - { + if ( + iter->second.type != ActionType::OVERALL && + !check(R_a, X_bar.predicates, X_bar.functions)) { if (t_2 != t_in) { ret.push_back(*iter); } else { @@ -710,9 +672,7 @@ STNBTBuilder::get_threat( } // Check if the input action and action k modify the same effect - if (action.second.type != ActionType::OVERALL && - iter->second.type != ActionType::OVERALL) - { + if (action.second.type != ActionType::OVERALL && iter->second.type != ActionType::OVERALL) { auto DX_hat = get_diff(X_1_k, X_hat); auto DX_bar = get_diff(X_1_a, X_bar); auto intersection = get_intersection(DX_hat, DX_bar); @@ -734,12 +694,9 @@ STNBTBuilder::get_threat( return ret; } -bool -STNBTBuilder::can_apply( - const std::pair & action, - const std::multimap & plan, - const int & time, - StateVec & state) const +bool STNBTBuilder::can_apply( + const std::pair & action, const std::multimap & plan, + const int & time, StateVec & state) const { auto X = state; auto R = get_conditions(action.second); @@ -777,10 +734,7 @@ STNBTBuilder::can_apply( return false; } -StateVec -STNBTBuilder::get_diff( - const StateVec & X_1, - const StateVec & X_2) const +StateVec STNBTBuilder::get_diff(const StateVec & X_1, const StateVec & X_2) const { StateVec ret; @@ -788,9 +742,7 @@ STNBTBuilder::get_diff( for (const auto & p_1 : X_1.predicates) { auto it = std::find_if( X_2.predicates.begin(), X_2.predicates.end(), - [&](plansys2::Predicate p_2) { - return parser::pddl::checkNodeEquality(p_1, p_2); - }); + [&](plansys2::Predicate p_2) { return parser::pddl::checkNodeEquality(p_1, p_2); }); if (it == X_2.predicates.end()) { ret.predicates.push_back(p_1); } @@ -800,9 +752,7 @@ STNBTBuilder::get_diff( for (const auto & p_2 : X_2.predicates) { auto it = std::find_if( X_1.predicates.begin(), X_1.predicates.end(), - [&](plansys2::Predicate p_1) { - return parser::pddl::checkNodeEquality(p_1, p_2); - }); + [&](plansys2::Predicate p_1) { return parser::pddl::checkNodeEquality(p_1, p_2); }); if (it == X_1.predicates.end()) { ret.predicates.push_back(p_2); } @@ -810,15 +760,13 @@ STNBTBuilder::get_diff( // Look for function changes for (const auto & f_1 : X_1.functions) { - auto it = std::find_if( - X_2.functions.begin(), X_2.functions.end(), - [&](plansys2::Function f_2) { - return parser::pddl::checkNodeEquality(f_1, f_2); - }); + auto it = std::find_if(X_2.functions.begin(), X_2.functions.end(), [&](plansys2::Function f_2) { + return parser::pddl::checkNodeEquality(f_1, f_2); + }); if (it != X_2.functions.end()) { - if (std::abs(f_1.value - it->value) > - 1e-5 * std::max(std::abs(f_1.value), std::abs(it->value))) - { + if ( + std::abs(f_1.value - it->value) > + 1e-5 * std::max(std::abs(f_1.value), std::abs(it->value))) { ret.functions.push_back(f_1); } } @@ -827,10 +775,7 @@ STNBTBuilder::get_diff( return ret; } -StateVec -STNBTBuilder::get_intersection( - const StateVec & X_1, - const StateVec & X_2) const +StateVec STNBTBuilder::get_intersection(const StateVec & X_1, const StateVec & X_2) const { StateVec ret; @@ -838,9 +783,7 @@ STNBTBuilder::get_intersection( for (const auto & p_1 : X_1.predicates) { auto it = std::find_if( X_2.predicates.begin(), X_2.predicates.end(), - [&](plansys2::Predicate p_2) { - return parser::pddl::checkNodeEquality(p_1, p_2); - }); + [&](plansys2::Predicate p_2) { return parser::pddl::checkNodeEquality(p_1, p_2); }); if (it != X_2.predicates.end()) { ret.predicates.push_back(p_1); } @@ -848,11 +791,9 @@ STNBTBuilder::get_intersection( // Look for functions in X_1 that are also in X_2 for (const auto & f_1 : X_1.functions) { - auto it = std::find_if( - X_2.functions.begin(), X_2.functions.end(), - [&](plansys2::Function f_2) { - return parser::pddl::checkNodeEquality(f_1, f_2); - }); + auto it = std::find_if(X_2.functions.begin(), X_2.functions.end(), [&](plansys2::Function f_2) { + return parser::pddl::checkNodeEquality(f_1, f_2); + }); if (it != X_2.functions.end()) { ret.functions.push_back(f_1); } @@ -861,8 +802,7 @@ STNBTBuilder::get_intersection( return ret; } -plansys2_msgs::msg::Tree -STNBTBuilder::get_conditions(const ActionStamped & action) const +plansys2_msgs::msg::Tree STNBTBuilder::get_conditions(const ActionStamped & action) const { if (action.type == ActionType::START || action.type == ActionType::GOAL) { return action.action->at_start_requirements; @@ -875,8 +815,7 @@ STNBTBuilder::get_conditions(const ActionStamped & action) const return plansys2_msgs::msg::Tree(); } -plansys2_msgs::msg::Tree -STNBTBuilder::get_effects(const ActionStamped & action) const +plansys2_msgs::msg::Tree STNBTBuilder::get_effects(const ActionStamped & action) const { if (action.type == ActionType::START) { return action.action->at_start_effects; @@ -887,8 +826,7 @@ STNBTBuilder::get_effects(const ActionStamped & action) const return plansys2_msgs::msg::Tree(); } -void -STNBTBuilder::prune_paths(GraphNode::Ptr current, GraphNode::Ptr previous) const +void STNBTBuilder::prune_paths(GraphNode::Ptr current, GraphNode::Ptr previous) const { // Traverse the graph from the previous node to the root for (auto & in : previous->input_arcs) { @@ -931,8 +869,7 @@ STNBTBuilder::prune_paths(GraphNode::Ptr current, GraphNode::Ptr previous) const } } -bool -STNBTBuilder::check_paths(GraphNode::Ptr current, GraphNode::Ptr previous) const +bool STNBTBuilder::check_paths(GraphNode::Ptr current, GraphNode::Ptr previous) const { // Traverse the graph from the current node to the root for (auto & in : current->input_arcs) { @@ -949,11 +886,8 @@ STNBTBuilder::check_paths(GraphNode::Ptr current, GraphNode::Ptr previous) const return false; } -std::string -STNBTBuilder::get_flow( - const GraphNode::Ptr node, - const GraphNode::Ptr parent, - std::set & used, +std::string STNBTBuilder::get_flow( + const GraphNode::Ptr node, const GraphNode::Ptr parent, std::set & used, const int & level) const { int l = level; @@ -987,9 +921,8 @@ STNBTBuilder::get_flow( int n = 0; if (node->output_arcs.size() > 1) { - flow = flow + t(l + 1) + - "output_arcs.size()) + - "\" failure_threshold=\"1\">\n"; + flow = flow + t(l + 1) + "output_arcs.size()) + "\" failure_threshold=\"1\">\n"; n = 1; } @@ -1023,11 +956,8 @@ STNBTBuilder::get_flow( return flow; } -std::string -STNBTBuilder::start_execution_block( - const GraphNode::Ptr node, - const GraphNode::Ptr parent, - const int & l) const +std::string STNBTBuilder::start_execution_block( + const GraphNode::Ptr node, const GraphNode::Ptr parent, const int & l) const { std::string ret; std::string ret_aux = bt_start_action_; @@ -1038,7 +968,7 @@ STNBTBuilder::start_execution_block( const auto & prev_node = std::get<0>(prev); if (prev_node != parent) { wait_actions = wait_actions + t(1) + "action, action_time_precision_) + "\"/>"; + to_action_id(prev_node->action, action_time_precision_) + "\"/>"; } if (prev != *node->input_arcs.rbegin()) { @@ -1059,11 +989,8 @@ STNBTBuilder::start_execution_block( return ret; } -std::string -STNBTBuilder::end_execution_block( - const GraphNode::Ptr node, - const GraphNode::Ptr parent, - const int & l) const +std::string STNBTBuilder::end_execution_block( + const GraphNode::Ptr node, const GraphNode::Ptr parent, const int & l) const { std::string ret; std::string ret_aux = bt_end_action_; @@ -1074,7 +1001,7 @@ STNBTBuilder::end_execution_block( const auto & prev_node = std::get<0>(prev); if (prev_node != parent) { check_actions = check_actions + t(1) + "action, action_time_precision_) + "\"/>"; + to_action_id(prev_node->action, action_time_precision_) + "\"/>"; } if (prev != *node->input_arcs.rbegin()) { @@ -1095,24 +1022,19 @@ STNBTBuilder::end_execution_block( return ret; } -void -STNBTBuilder::get_flow_dotgraph( - GraphNode::Ptr node, - std::set & edges) +void STNBTBuilder::get_flow_dotgraph(GraphNode::Ptr node, std::set & edges) { for (const auto & arc : node->output_arcs) { auto child = std::get<0>(arc); - std::string edge = std::to_string(node->node_num) + "->" + std::to_string(child->node_num) + - ";\n"; + std::string edge = + std::to_string(node->node_num) + "->" + std::to_string(child->node_num) + ";\n"; edges.insert(edge); get_flow_dotgraph(child, edges); } } -std::string -STNBTBuilder::get_node_dotgraph( - GraphNode::Ptr node, - std::shared_ptr> action_map) +std::string STNBTBuilder::get_node_dotgraph( + GraphNode::Ptr node, std::shared_ptr> action_map) { std::stringstream ss; ss << t(2) << node->node_num << " [label=\""; @@ -1144,13 +1066,12 @@ STNBTBuilder::get_node_dotgraph( return ss.str(); } -ActionExecutor::Status -STNBTBuilder::get_action_status( +ActionExecutor::Status STNBTBuilder::get_action_status( ActionStamped action_stamped, std::shared_ptr> action_map) { - auto index = "(" + parser::pddl::nameActionsToString(action_stamped.action) + "):" + - std::to_string(static_cast(action_stamped.time * 1000)); + auto index = "(" + parser::pddl::nameActionsToString(action_stamped.action) + + "):" + std::to_string(static_cast(action_stamped.time * 1000)); if (action_map->find(index) != action_map->end()) { if ((*action_map)[index].action_executor) { return (*action_map)[index].action_executor->get_internal_status(); @@ -1162,10 +1083,7 @@ STNBTBuilder::get_action_status( } } -std::string -STNBTBuilder::add_dot_graph_legend( - int level_counter, - int node_counter) +std::string STNBTBuilder::add_dot_graph_legend(int level_counter, int node_counter) { std::stringstream ss; int legend_counter = level_counter; @@ -1188,17 +1106,18 @@ STNBTBuilder::add_dot_graph_legend( ss << t(3); ss << "labeljust = l;\n"; ss << t(3); - ss << legend_node_counter++ << - " [label=\n\"Finished action\n\",labeljust=c,style=filled,color=green4,fillcolor=seagreen2];\n"; + ss << legend_node_counter++ + << " [label=\n\"Finished " + "action\n\",labeljust=c,style=filled,color=green4,fillcolor=seagreen2];\n"; ss << t(3); - ss << legend_node_counter++ << - " [label=\n\"Failed action\n\",labeljust=c,style=filled,color=red,fillcolor=pink];\n"; + ss << legend_node_counter++ + << " [label=\n\"Failed action\n\",labeljust=c,style=filled,color=red,fillcolor=pink];\n"; ss << t(3); - ss << legend_node_counter++ << - " [label=\n\"Current action\n\",labeljust=c,style=filled,color=blue,fillcolor=skyblue];\n"; + ss << legend_node_counter++ + << " [label=\n\"Current action\n\",labeljust=c,style=filled,color=blue,fillcolor=skyblue];\n"; ss << t(3); - ss << legend_node_counter++ << " [label=\n\"Future action\n\",labeljust=c,style=filled," << - "color=yellow3,fillcolor=lightgoldenrod1];\n"; + ss << legend_node_counter++ << " [label=\n\"Future action\n\",labeljust=c,style=filled," + << "color=yellow3,fillcolor=lightgoldenrod1];\n"; ss << t(2); ss << "}\n"; @@ -1217,14 +1136,12 @@ STNBTBuilder::add_dot_graph_legend( return ss.str(); } -void -STNBTBuilder::print_graph(const plansys2::Graph::Ptr graph) const +void STNBTBuilder::print_graph(const plansys2::Graph::Ptr graph) const { print_node(graph->nodes.front(), 0); } -void -STNBTBuilder::print_node(const plansys2::GraphNode::Ptr node, int level) const +void STNBTBuilder::print_node(const plansys2::GraphNode::Ptr node, int level) const { std::cerr << t(level) << "(" << node->node_num << ") "; if (node->action.type == ActionType::START) { @@ -1251,11 +1168,8 @@ STNBTBuilder::print_node(const plansys2::GraphNode::Ptr node, int level) const } } -void -STNBTBuilder::replace( - std::string & str, - const std::string & from, - const std::string & to) const +void STNBTBuilder::replace( + std::string & str, const std::string & from, const std::string & to) const { size_t start_pos = std::string::npos; while ((start_pos = str.find(from)) != std::string::npos) { @@ -1263,21 +1177,17 @@ STNBTBuilder::replace( } } -bool -STNBTBuilder::is_end( - const std::tuple & edge, - const ActionStamped & action) const +bool STNBTBuilder::is_end( + const std::tuple & edge, const ActionStamped & action) const { const auto & node = std::get<0>(edge); auto t_1 = to_int_time(node->action.time, action_time_precision_ + 1); auto t_2 = to_int_time(action.time, action_time_precision_ + 1); - return action.type == ActionType::START && - node->action.type == ActionType::END && - (t_1 == t_2) && (node->action.expression == action.expression); + return action.type == ActionType::START && node->action.type == ActionType::END && (t_1 == t_2) && + (node->action.expression == action.expression); } -std::string -STNBTBuilder::t(const int & level) const +std::string STNBTBuilder::t(const int & level) const { std::string ret; for (int i = 0; i < level; i++) { diff --git a/plansys2_executor/test/pddl/cooking_domain.pddl b/plansys2_executor/test/pddl/cooking_domain.pddl index 95b9d33e..0932f44b 100644 --- a/plansys2_executor/test/pddl/cooking_domain.pddl +++ b/plansys2_executor/test/pddl/cooking_domain.pddl @@ -5,7 +5,7 @@ (:types ingredient dish -zone +zone robot );; end Types ;;;;;;;;;;;;;;;;;;;;;;;;; @@ -22,11 +22,11 @@ robot (is_flour ?i - ingredient) (is_sugar ?i - ingredient) (is_pasta ?i - ingredient) -(is_water ?i - ingredient) +(is_water ?i - ingredient) -(is_omelette ?d - dish) -(is_cake ?d - dish) -(is_spaghetti ?d - dish) +(is_omelette ?d - dish) +(is_cake ?d - dish) +(is_spaghetti ?d - dish) (is_cooking_zone ?z - zone) (is_pantry_zone ?z - zone) diff --git a/plansys2_executor/test/pddl/domain_charging.pddl b/plansys2_executor/test/pddl/domain_charging.pddl index 5a29ecdb..c68ed811 100644 --- a/plansys2_executor/test/pddl/domain_charging.pddl +++ b/plansys2_executor/test/pddl/domain_charging.pddl @@ -35,7 +35,7 @@ ;; condition - at start -> ?wp1 (waypoint) is connected to ?wp2 (waypoint) ;; at start -> ?r (robot) is at ?wp1 (waypoint) ;; at start -> state_of_charge(?r) >= distance(?wp1, ?wp2) / max_range(?r) - ;; efect - at start -> ?r (robot) is NOT at ?wp1 (waypoint) + ;; effect - at start -> ?r (robot) is NOT at ?wp1 (waypoint) ;; at start -> state_of_charge(?r) -= distance(?wp1, ?wp2) / max_range(?r) ;; at end -> ?r (robot) is at ?wp2 (waypoint) (:durative-action move @@ -55,7 +55,7 @@ ;; Patrol ?wp (waypoint) with ?r (robot). ;; duration = 5 ;; condition - at start -> ?r (robot) is at ?wp (waypoint) - ;; efect - at end -> ?wp (waypoint) has been patrolled + ;; effect - at end -> ?wp (waypoint) has been patrolled (:durative-action patrol :parameters (?r - robot ?wp - waypoint) :duration (= ?duration 5) @@ -67,7 +67,7 @@ ;; duration = 5 ;; condition - at start -> ?r (robot) is at ?wp (waypoint) ;; at start -> ?wp (waypoint) has a charger - ;; efect - at end -> state_of_charge(?r) = 100 + ;; effect - at end -> state_of_charge(?r) = 100 (:durative-action charge :parameters (?r - robot ?wp - waypoint) :duration (= ?duration 5) diff --git a/plansys2_executor/test/pddl/elevator_domain.pddl b/plansys2_executor/test/pddl/elevator_domain.pddl index 13edaf8e..fb23edfa 100644 --- a/plansys2_executor/test/pddl/elevator_domain.pddl +++ b/plansys2_executor/test/pddl/elevator_domain.pddl @@ -32,7 +32,7 @@ ;; duration = floor_distance(?cur, ?nxt) / elevator_speed(?lift) ;; condition - at start -> ?lift (elevator) is at ?cur (num) ;; over all -> ?nxt (num) is next after ?cur (num) - ;; efect - at start -> ?lift (elevator) is NOT at ?cur (num) + ;; effect - at start -> ?lift (elevator) is NOT at ?cur (num) ;; at end -> ?lift (elevator is at ?nxt (num) (:durative-action move-up :parameters (?lift - elevator ?cur ?nxt - num) @@ -48,7 +48,7 @@ ;; duration = floor_distance(?cur, ?nxt) / elevator_speed(?lift) ;; condition - at start -> ?lift (elevator) is at ?cur (num) ;; over all -> ?cur (num) is next after ?nxt (num) - ;; efect - at start -> ?lift (elevator) is NOT at ?cur (num) + ;; effect - at start -> ?lift (elevator) is NOT at ?cur (num) ;; at end -> ?lift (elevator is at ?nxt (num) (:durative-action move-down :parameters (?lift - elevator ?cur ?nxt - num) diff --git a/plansys2_executor/test/pddl/problem_simple_1.pddl b/plansys2_executor/test/pddl/problem_simple_1.pddl index adb76c85..2de36ca2 100644 --- a/plansys2_executor/test/pddl/problem_simple_1.pddl +++ b/plansys2_executor/test/pddl/problem_simple_1.pddl @@ -15,7 +15,7 @@ ;; The goal is to have both packages delivered to their destinations: (:goal (and - (robot_talk leia m1 Jack) + (robot_talk leia m1 Jack) ) ) ) diff --git a/plansys2_executor/test/pddl/problem_simple_2.pddl b/plansys2_executor/test/pddl/problem_simple_2.pddl index dfc51ee1..05fb33a7 100644 --- a/plansys2_executor/test/pddl/problem_simple_2.pddl +++ b/plansys2_executor/test/pddl/problem_simple_2.pddl @@ -14,7 +14,7 @@ ;; The goal is to have both packages delivered to their destinations: (:goal (and - (robot_talk leia m1 Jack) + (robot_talk leia m1 Jack) ) ) ) diff --git a/plansys2_executor/test/pddl/problem_simple_3.pddl b/plansys2_executor/test/pddl/problem_simple_3.pddl index 056ec3e6..21cc29e2 100644 --- a/plansys2_executor/test/pddl/problem_simple_3.pddl +++ b/plansys2_executor/test/pddl/problem_simple_3.pddl @@ -12,7 +12,7 @@ ;; The goal is to have both packages delivered to their destinations: (:goa (and - (robot_talk leia m1 Jack) - + (robot_talk leia m1 Jack) + ) ) diff --git a/plansys2_executor/test/unit/action_execution_test.cpp b/plansys2_executor/test/unit/action_execution_test.cpp index ce86dd6c..7593e1d4 100644 --- a/plansys2_executor/test/unit/action_execution_test.cpp +++ b/plansys2_executor/test/unit/action_execution_test.cpp @@ -12,54 +12,45 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include -#include -#include #include +#include #include +#include +#include +#include +#include #include "ament_index_cpp/get_package_share_directory.hpp" - -#include "plansys2_domain_expert/DomainExpertNode.hpp" +#include "behaviortree_cpp_v3/behavior_tree.h" +#include "behaviortree_cpp_v3/blackboard.h" +#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp_v3/utils/shared_library.h" +#include "gtest/gtest.h" +#include "lifecycle_msgs/msg/state.hpp" #include "plansys2_domain_expert/DomainExpertClient.hpp" -#include "plansys2_problem_expert/ProblemExpertNode.hpp" -#include "plansys2_problem_expert/ProblemExpertClient.hpp" -#include "plansys2_planner/PlannerNode.hpp" -#include "plansys2_planner/PlannerClient.hpp" -#include "plansys2_executor/BTBuilder.hpp" - +#include "plansys2_domain_expert/DomainExpertNode.hpp" #include "plansys2_executor/ActionExecutor.hpp" #include "plansys2_executor/ActionExecutorClient.hpp" -#include "plansys2_executor/ExecutorNode.hpp" +#include "plansys2_executor/BTBuilder.hpp" #include "plansys2_executor/ExecutorClient.hpp" -#include "plansys2_problem_expert/Utils.hpp" - -#include "behaviortree_cpp_v3/behavior_tree.h" -#include "behaviortree_cpp_v3/bt_factory.h" -#include "behaviortree_cpp_v3/utils/shared_library.h" -#include "behaviortree_cpp_v3/blackboard.h" - +#include "plansys2_executor/ExecutorNode.hpp" +#include "plansys2_executor/behavior_tree/apply_atend_effect_node.hpp" +#include "plansys2_executor/behavior_tree/apply_atstart_effect_node.hpp" +#include "plansys2_executor/behavior_tree/check_atend_req_node.hpp" +#include "plansys2_executor/behavior_tree/check_overall_req_node.hpp" #include "plansys2_executor/behavior_tree/execute_action_node.hpp" #include "plansys2_executor/behavior_tree/wait_action_node.hpp" #include "plansys2_executor/behavior_tree/wait_atstart_req_node.hpp" -#include "plansys2_executor/behavior_tree/check_overall_req_node.hpp" -#include "plansys2_executor/behavior_tree/check_atend_req_node.hpp" -#include "plansys2_executor/behavior_tree/apply_atstart_effect_node.hpp" -#include "plansys2_executor/behavior_tree/apply_atend_effect_node.hpp" - -#include "lifecycle_msgs/msg/state.hpp" - +#include "plansys2_planner/PlannerClient.hpp" +#include "plansys2_planner/PlannerNode.hpp" +#include "plansys2_problem_expert/ProblemExpertClient.hpp" +#include "plansys2_problem_expert/ProblemExpertNode.hpp" +#include "plansys2_problem_expert/Utils.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "gtest/gtest.h" - - -using CallbackReturnT = - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; +using CallbackReturnT = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; using namespace std::chrono_literals; class MoveAction : public plansys2::ActionExecutorClient @@ -71,7 +62,6 @@ class MoveAction : public plansys2::ActionExecutorClient return std::make_shared(node_name, rate); } - MoveAction(const std::string & id, const std::chrono::nanoseconds & rate) : ActionExecutorClient(id, rate) { @@ -79,8 +69,7 @@ class MoveAction : public plansys2::ActionExecutorClient cycles_ = 0; } - CallbackReturnT - on_activate(const rclcpp_lifecycle::State & state) + CallbackReturnT on_activate(const rclcpp_lifecycle::State & state) { std::cerr << "MoveAction::on_activate" << std::endl; counter_ = 0; @@ -141,8 +130,10 @@ TEST(action_execution, protocol_basic) bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} - }); + while (!finish) { + exe.spin_some(); + } + }); ASSERT_EQ(move_action_executor->get_internal_status(), plansys2::ActionExecutor::Status::IDLE); ASSERT_EQ( @@ -161,8 +152,7 @@ TEST(action_execution, protocol_basic) } ASSERT_EQ( - move_action_node->get_current_state().id(), - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + move_action_node->get_current_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); ASSERT_EQ( move_action_node->get_internal_status().state, @@ -207,7 +197,6 @@ TEST(action_execution, protocol_basic) move_action_node->get_internal_status().state, plansys2_msgs::msg::ActionPerformerStatus::READY); - ASSERT_EQ(action_execution_msgs.size(), 8u); ASSERT_EQ(action_execution_msgs[3].type, plansys2_msgs::msg::ActionExecution::FEEDBACK); ASSERT_EQ(action_execution_msgs[4].type, plansys2_msgs::msg::ActionExecution::FEEDBACK); @@ -215,7 +204,6 @@ TEST(action_execution, protocol_basic) ASSERT_EQ(action_execution_msgs[6].type, plansys2_msgs::msg::ActionExecution::FEEDBACK); ASSERT_EQ(action_execution_msgs[7].type, plansys2_msgs::msg::ActionExecution::FINISH); - ASSERT_EQ(move_action_executor->get_internal_status(), plansys2::ActionExecutor::Status::SUCCESS); ASSERT_EQ( move_action_node->get_internal_status().state, @@ -256,8 +244,10 @@ TEST(action_execution, protocol_cancelation) bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} - }); + while (!finish) { + exe.spin_some(); + } + }); ASSERT_EQ(move_action_executor->get_internal_status(), plansys2::ActionExecutor::Status::IDLE); ASSERT_EQ( @@ -276,8 +266,7 @@ TEST(action_execution, protocol_cancelation) } ASSERT_EQ( - move_action_node->get_current_state().id(), - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + move_action_node->get_current_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); ASSERT_EQ( move_action_node->get_internal_status().state, @@ -298,8 +287,7 @@ TEST(action_execution, protocol_cancelation) ASSERT_EQ( move_action_node->get_current_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); - ASSERT_EQ( - move_action_executor->get_internal_status(), plansys2::ActionExecutor::Status::RUNNING); + ASSERT_EQ(move_action_executor->get_internal_status(), plansys2::ActionExecutor::Status::RUNNING); ASSERT_EQ( move_action_node->get_internal_status().state, plansys2_msgs::msg::ActionPerformerStatus::RUNNING); @@ -330,13 +318,11 @@ TEST(action_execution, protocol_cancelation) } ASSERT_EQ( - move_action_executor->get_internal_status(), - plansys2::ActionExecutor::Status::CANCELLED); + move_action_executor->get_internal_status(), plansys2::ActionExecutor::Status::CANCELLED); ASSERT_EQ( move_action_node->get_internal_status().state, plansys2_msgs::msg::ActionPerformerStatus::READY); - ASSERT_EQ(action_execution_msgs.size(), 6u); ASSERT_EQ(action_execution_msgs[3].type, plansys2_msgs::msg::ActionExecution::FEEDBACK); ASSERT_EQ(action_execution_msgs[4].type, plansys2_msgs::msg::ActionExecution::FEEDBACK); diff --git a/plansys2_executor/test/unit/bt_node_test.cpp b/plansys2_executor/test/unit/bt_node_test.cpp index f1a51173..59aa2356 100644 --- a/plansys2_executor/test/unit/bt_node_test.cpp +++ b/plansys2_executor/test/unit/bt_node_test.cpp @@ -12,51 +12,43 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include -#include -#include #include +#include #include +#include +#include +#include +#include #include "ament_index_cpp/get_package_share_directory.hpp" - -#include "plansys2_domain_expert/DomainExpertNode.hpp" -#include "plansys2_domain_expert/DomainExpertClient.hpp" -#include "plansys2_problem_expert/ProblemExpertNode.hpp" -#include "plansys2_problem_expert/ProblemExpertClient.hpp" -#include "plansys2_planner/PlannerNode.hpp" -#include "plansys2_planner/PlannerClient.hpp" -#include "plansys2_executor/BTBuilder.hpp" - -#include "plansys2_executor/ActionExecutor.hpp" -#include "plansys2_executor/ActionExecutorClient.hpp" -#include "plansys2_problem_expert/Utils.hpp" -#include "plansys2_pddl_parser/Utils.h" - #include "behaviortree_cpp_v3/behavior_tree.h" +#include "behaviortree_cpp_v3/blackboard.h" #include "behaviortree_cpp_v3/bt_factory.h" #include "behaviortree_cpp_v3/utils/shared_library.h" -#include "behaviortree_cpp_v3/blackboard.h" - +#include "gtest/gtest.h" +#include "lifecycle_msgs/msg/state.hpp" +#include "plansys2_domain_expert/DomainExpertClient.hpp" +#include "plansys2_domain_expert/DomainExpertNode.hpp" +#include "plansys2_executor/ActionExecutor.hpp" +#include "plansys2_executor/ActionExecutorClient.hpp" +#include "plansys2_executor/BTBuilder.hpp" +#include "plansys2_executor/behavior_tree/apply_atend_effect_node.hpp" +#include "plansys2_executor/behavior_tree/apply_atstart_effect_node.hpp" +#include "plansys2_executor/behavior_tree/check_atend_req_node.hpp" +#include "plansys2_executor/behavior_tree/check_overall_req_node.hpp" #include "plansys2_executor/behavior_tree/execute_action_node.hpp" #include "plansys2_executor/behavior_tree/wait_action_node.hpp" #include "plansys2_executor/behavior_tree/wait_atstart_req_node.hpp" -#include "plansys2_executor/behavior_tree/check_overall_req_node.hpp" -#include "plansys2_executor/behavior_tree/check_atend_req_node.hpp" -#include "plansys2_executor/behavior_tree/apply_atstart_effect_node.hpp" -#include "plansys2_executor/behavior_tree/apply_atend_effect_node.hpp" - -#include "lifecycle_msgs/msg/state.hpp" - +#include "plansys2_pddl_parser/Utils.h" +#include "plansys2_planner/PlannerClient.hpp" +#include "plansys2_planner/PlannerNode.hpp" +#include "plansys2_problem_expert/ProblemExpertClient.hpp" +#include "plansys2_problem_expert/ProblemExpertNode.hpp" +#include "plansys2_problem_expert/Utils.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "gtest/gtest.h" - - TEST(problem_expert, wait_overall_req_test) { auto test_node = rclcpp::Node::make_shared("test_node"); @@ -79,9 +71,10 @@ TEST(problem_expert, wait_overall_req_test) bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} - }); - + while (!finish) { + exe.spin_some(); + } + }); domain_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); problem_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); @@ -109,12 +102,11 @@ TEST(problem_expert, wait_overall_req_test) (*action_map)["(move robot1 wheels_zone assembly_zone):5"] = plansys2::ActionExecutionInfo(); (*action_map)["(move robot1 wheels_zone assembly_zone):5"].durative_action_info = domain_client->getDurativeAction( - plansys2::get_action_name("(move robot1 wheels_zone assembly_zone)"), - plansys2::get_action_params("(move robot1 wheels_zone assembly_zone)")); + plansys2::get_action_name("(move robot1 wheels_zone assembly_zone)"), + plansys2::get_action_params("(move robot1 wheels_zone assembly_zone)")); ASSERT_NE( - (*action_map)["(move robot1 wheels_zone assembly_zone):5"].durative_action_info, - nullptr); + (*action_map)["(move robot1 wheels_zone assembly_zone):5"].durative_action_info, nullptr); std::string bt_xml_tree = R"( @@ -137,15 +129,13 @@ TEST(problem_expert, wait_overall_req_test) factory.registerNodeType("ExecuteAction"); factory.registerNodeType("CheckOverAllReq"); - ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("robot1", "robot"))); ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("wheels_zone", "zone"))); ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("assembly_zone", "zone"))); std::vector predicates = { - "(robot_available robot1)", - "(robot_at robot1 wheels_zone)"}; + "(robot_available robot1)", "(robot_at robot1 wheels_zone)"}; try { auto tree = factory.createTreeFromText(bt_xml_tree, blackboard); @@ -193,9 +183,10 @@ TEST(problem_expert, wait_atstart_req_test) bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} - }); - + while (!finish) { + exe.spin_some(); + } + }); domain_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); problem_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); @@ -223,12 +214,11 @@ TEST(problem_expert, wait_atstart_req_test) (*action_map)["(move robot1 wheels_zone assembly_zone):5"] = plansys2::ActionExecutionInfo(); (*action_map)["(move robot1 wheels_zone assembly_zone):5"].durative_action_info = domain_client->getDurativeAction( - plansys2::get_action_name("(move robot1 wheels_zone assembly_zone)"), - plansys2::get_action_params("(move robot1 wheels_zone assembly_zone)")); + plansys2::get_action_name("(move robot1 wheels_zone assembly_zone)"), + plansys2::get_action_params("(move robot1 wheels_zone assembly_zone)")); ASSERT_NE( - (*action_map)["(move robot1 wheels_zone assembly_zone):5"].durative_action_info, - nullptr); + (*action_map)["(move robot1 wheels_zone assembly_zone):5"].durative_action_info, nullptr); std::string bt_xml_tree = R"( @@ -251,15 +241,13 @@ TEST(problem_expert, wait_atstart_req_test) factory.registerNodeType("ExecuteAction"); factory.registerNodeType("WaitAtStartReq"); - ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("robot1", "robot"))); ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("wheels_zone", "zone"))); ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("assembly_zone", "zone"))); std::vector predicates = { - "(robot_available robot1)", - "(robot_at robot1 wheels_zone)"}; + "(robot_available robot1)", "(robot_at robot1 wheels_zone)"}; try { auto tree = factory.createTreeFromText(bt_xml_tree, blackboard); @@ -272,7 +260,6 @@ TEST(problem_expert, wait_atstart_req_test) status = tree.tickRoot(); ASSERT_EQ(status, BT::NodeStatus::RUNNING); - for (const auto & pred : predicates) { ASSERT_TRUE(problem_client->addPredicate(plansys2::Predicate(pred))); } @@ -309,9 +296,10 @@ TEST(problem_expert, wait_atend_req_test) bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} - }); - + while (!finish) { + exe.spin_some(); + } + }); domain_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); problem_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); @@ -339,12 +327,11 @@ TEST(problem_expert, wait_atend_req_test) (*action_map)["(move robot1 wheels_zone assembly_zone):5"] = plansys2::ActionExecutionInfo(); (*action_map)["(move robot1 wheels_zone assembly_zone):5"].durative_action_info = domain_client->getDurativeAction( - plansys2::get_action_name("(move robot1 wheels_zone assembly_zone)"), - plansys2::get_action_params("(move robot1 wheels_zone assembly_zone)")); + plansys2::get_action_name("(move robot1 wheels_zone assembly_zone)"), + plansys2::get_action_params("(move robot1 wheels_zone assembly_zone)")); ASSERT_NE( - (*action_map)["(move robot1 wheels_zone assembly_zone):5"].durative_action_info, - nullptr); + (*action_map)["(move robot1 wheels_zone assembly_zone):5"].durative_action_info, nullptr); std::string bt_xml_tree = R"( @@ -367,15 +354,13 @@ TEST(problem_expert, wait_atend_req_test) factory.registerNodeType("ExecuteAction"); factory.registerNodeType("CheckAtEndReq"); - ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("robot1", "robot"))); ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("wheels_zone", "zone"))); ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("assembly_zone", "zone"))); std::vector predicates = { - "(robot_available robot1)", - "(robot_at robot1 wheels_zone)"}; + "(robot_available robot1)", "(robot_at robot1 wheels_zone)"}; try { auto tree = factory.createTreeFromText(bt_xml_tree, blackboard); @@ -423,9 +408,10 @@ TEST(problem_expert, at_start_effect_test) bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} - }); - + while (!finish) { + exe.spin_some(); + } + }); domain_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); problem_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); @@ -453,12 +439,11 @@ TEST(problem_expert, at_start_effect_test) (*action_map)["(move robot1 wheels_zone assembly_zone):5"] = plansys2::ActionExecutionInfo(); (*action_map)["(move robot1 wheels_zone assembly_zone):5"].durative_action_info = domain_client->getDurativeAction( - plansys2::get_action_name("(move robot1 wheels_zone assembly_zone)"), - plansys2::get_action_params("(move robot1 wheels_zone assembly_zone)")); + plansys2::get_action_name("(move robot1 wheels_zone assembly_zone)"), + plansys2::get_action_params("(move robot1 wheels_zone assembly_zone)")); ASSERT_NE( - (*action_map)["(move robot1 wheels_zone assembly_zone):5"].durative_action_info, - nullptr); + (*action_map)["(move robot1 wheels_zone assembly_zone):5"].durative_action_info, nullptr); std::string bt_xml_tree = R"( @@ -481,7 +466,6 @@ TEST(problem_expert, at_start_effect_test) factory.registerNodeType("ExecuteAction"); factory.registerNodeType("ApplyAtStartEffect"); - ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("robot1", "robot"))); ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("wheels_zone", "zone"))); @@ -489,8 +473,7 @@ TEST(problem_expert, at_start_effect_test) try { std::vector predicates = { - "(robot_available robot1)", - "(robot_at robot1 wheels_zone)"}; + "(robot_available robot1)", "(robot_at robot1 wheels_zone)"}; for (const auto & pred : predicates) { ASSERT_TRUE(problem_client->addPredicate(plansys2::Predicate(pred))); @@ -509,9 +492,7 @@ TEST(problem_expert, at_start_effect_test) } } ASSERT_FALSE( - problem_client->existPredicate( - plansys2::Predicate( - "(robot_at robot1 wheels_zone)"))); + problem_client->existPredicate(plansys2::Predicate("(robot_at robot1 wheels_zone)"))); } catch (std::exception & e) { std::cerr << e.what() << std::endl; } @@ -542,9 +523,10 @@ TEST(problem_expert, at_end_effect_test) bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} - }); - + while (!finish) { + exe.spin_some(); + } + }); domain_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); problem_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); @@ -572,12 +554,11 @@ TEST(problem_expert, at_end_effect_test) (*action_map)["(move robot1 wheels_zone assembly_zone):5"] = plansys2::ActionExecutionInfo(); (*action_map)["(move robot1 wheels_zone assembly_zone):5"].durative_action_info = domain_client->getDurativeAction( - plansys2::get_action_name("(move robot1 wheels_zone assembly_zone)"), - plansys2::get_action_params("(move robot1 wheels_zone assembly_zone)")); + plansys2::get_action_name("(move robot1 wheels_zone assembly_zone)"), + plansys2::get_action_params("(move robot1 wheels_zone assembly_zone)")); ASSERT_NE( - (*action_map)["(move robot1 wheels_zone assembly_zone):5"].durative_action_info, - nullptr); + (*action_map)["(move robot1 wheels_zone assembly_zone):5"].durative_action_info, nullptr); std::string bt_xml_tree = R"( @@ -600,15 +581,13 @@ TEST(problem_expert, at_end_effect_test) factory.registerNodeType("ExecuteAction"); factory.registerNodeType("ApplyAtEndEffect"); - ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("robot1", "robot"))); ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("wheels_zone", "zone"))); ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("assembly_zone", "zone"))); try { - std::vector predicates = { - "(robot_at robot1 wheels_zone)"}; + std::vector predicates = {"(robot_at robot1 wheels_zone)"}; for (const auto & pred : predicates) { ASSERT_TRUE(problem_client->addPredicate(plansys2::Predicate(pred))); @@ -628,9 +607,7 @@ TEST(problem_expert, at_end_effect_test) } ASSERT_TRUE( - problem_client->existPredicate( - plansys2::Predicate( - "(robot_at robot1 assembly_zone)"))); + problem_client->existPredicate(plansys2::Predicate("(robot_at robot1 assembly_zone)"))); } catch (std::exception & e) { std::cerr << e.what() << std::endl; } diff --git a/plansys2_executor/test/unit/bt_node_test_charging.cpp b/plansys2_executor/test/unit/bt_node_test_charging.cpp index 462471f0..5ac0dc99 100644 --- a/plansys2_executor/test/unit/bt_node_test_charging.cpp +++ b/plansys2_executor/test/unit/bt_node_test_charging.cpp @@ -12,46 +12,39 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include -#include -#include #include +#include #include +#include +#include +#include +#include #include "ament_index_cpp/get_package_share_directory.hpp" - -#include "plansys2_domain_expert/DomainExpertNode.hpp" -#include "plansys2_domain_expert/DomainExpertClient.hpp" -#include "plansys2_problem_expert/ProblemExpertNode.hpp" -#include "plansys2_problem_expert/ProblemExpertClient.hpp" -#include "plansys2_planner/PlannerNode.hpp" -#include "plansys2_planner/PlannerClient.hpp" -#include "plansys2_executor/BTBuilder.hpp" - -#include "plansys2_executor/ActionExecutor.hpp" -#include "plansys2_executor/ActionExecutorClient.hpp" -#include "plansys2_problem_expert/Utils.hpp" - #include "behaviortree_cpp_v3/behavior_tree.h" +#include "behaviortree_cpp_v3/blackboard.h" #include "behaviortree_cpp_v3/bt_factory.h" #include "behaviortree_cpp_v3/utils/shared_library.h" -#include "behaviortree_cpp_v3/blackboard.h" - +#include "gtest/gtest.h" +#include "lifecycle_msgs/msg/state.hpp" +#include "plansys2_domain_expert/DomainExpertClient.hpp" +#include "plansys2_domain_expert/DomainExpertNode.hpp" +#include "plansys2_executor/ActionExecutor.hpp" +#include "plansys2_executor/ActionExecutorClient.hpp" +#include "plansys2_executor/BTBuilder.hpp" +#include "plansys2_executor/behavior_tree/apply_atend_effect_node.hpp" +#include "plansys2_executor/behavior_tree/apply_atstart_effect_node.hpp" #include "plansys2_executor/behavior_tree/execute_action_node.hpp" #include "plansys2_executor/behavior_tree/wait_atstart_req_node.hpp" -#include "plansys2_executor/behavior_tree/apply_atstart_effect_node.hpp" -#include "plansys2_executor/behavior_tree/apply_atend_effect_node.hpp" - -#include "lifecycle_msgs/msg/state.hpp" - +#include "plansys2_planner/PlannerClient.hpp" +#include "plansys2_planner/PlannerNode.hpp" +#include "plansys2_problem_expert/ProblemExpertClient.hpp" +#include "plansys2_problem_expert/ProblemExpertNode.hpp" +#include "plansys2_problem_expert/Utils.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "gtest/gtest.h" - TEST(problem_expert, wait_atstart_req_test) { auto test_node = rclcpp::Node::make_shared("test_node"); @@ -74,9 +67,10 @@ TEST(problem_expert, wait_atstart_req_test) bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} - }); - + while (!finish) { + exe.spin_some(); + } + }); domain_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); problem_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); @@ -102,14 +96,11 @@ TEST(problem_expert, wait_atstart_req_test) auto action_map = std::make_shared>(); (*action_map)["(move robot1 wp1 wp2):5"] = plansys2::ActionExecutionInfo(); - (*action_map)["(move robot1 wp1 wp2):5"].durative_action_info = - domain_client->getDurativeAction( + (*action_map)["(move robot1 wp1 wp2):5"].durative_action_info = domain_client->getDurativeAction( plansys2::get_action_name("(move robot1 wp1 wp2)"), plansys2::get_action_params("(move robot1 wp1 wp2)")); - ASSERT_NE( - (*action_map)["(move robot1 wp1 wp2):5"].durative_action_info, - nullptr); + ASSERT_NE((*action_map)["(move robot1 wp1 wp2):5"].durative_action_info, nullptr); std::string bt_xml_tree = R"( @@ -137,16 +128,11 @@ TEST(problem_expert, wait_atstart_req_test) ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("wp2", "waypoint"))); std::vector predicates = { - "(robot_at robot1 wp1)", - "(charger_at wp2)", - "(connected wp1 wp2)"}; + "(robot_at robot1 wp1)", "(charger_at wp2)", "(connected wp1 wp2)"}; std::vector functions = { - "(= (speed robot1) 3)", - "(= (max_range robot1) 75)", - "(= (state_of_charge robot1) 99)", - "(= (distance wp1 wp2) 15)", - "(= (distance wp2 wp1) 15)"}; + "(= (speed robot1) 3)", "(= (max_range robot1) 75)", "(= (state_of_charge robot1) 99)", + "(= (distance wp1 wp2) 15)", "(= (distance wp2 wp1) 15)"}; try { auto tree = factory.createTreeFromText(bt_xml_tree, blackboard); @@ -199,9 +185,10 @@ TEST(problem_expert, apply_atstart_effect_test) bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} - }); - + while (!finish) { + exe.spin_some(); + } + }); domain_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); problem_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); @@ -227,14 +214,11 @@ TEST(problem_expert, apply_atstart_effect_test) auto action_map = std::make_shared>(); (*action_map)["(move robot1 wp1 wp2):5"] = plansys2::ActionExecutionInfo(); - (*action_map)["(move robot1 wp1 wp2):5"].durative_action_info = - domain_client->getDurativeAction( + (*action_map)["(move robot1 wp1 wp2):5"].durative_action_info = domain_client->getDurativeAction( plansys2::get_action_name("(move robot1 wp1 wp2)"), plansys2::get_action_params("(move robot1 wp1 wp2)")); - ASSERT_NE( - (*action_map)["(move robot1 wp1 wp2):5"].durative_action_info, - nullptr); + ASSERT_NE((*action_map)["(move robot1 wp1 wp2):5"].durative_action_info, nullptr); std::string bt_xml_tree = R"( @@ -263,20 +247,15 @@ TEST(problem_expert, apply_atstart_effect_test) try { std::vector predicates = { - "(robot_at robot1 wp1)", - "(charger_at wp2)", - "(connected wp1 wp2)"}; + "(robot_at robot1 wp1)", "(charger_at wp2)", "(connected wp1 wp2)"}; for (const auto & pred : predicates) { ASSERT_TRUE(problem_client->addPredicate(plansys2::Predicate(pred))); } std::vector functions = { - "(= (speed robot1) 3)", - "(= (max_range robot1) 75)", - "(= (state_of_charge robot1) 99)", - "(= (distance wp1 wp2) 15)", - "(= (distance wp2 wp1) 15)"}; + "(= (speed robot1) 3)", "(= (max_range robot1) 75)", "(= (state_of_charge robot1) 99)", + "(= (distance wp1 wp2) 15)", "(= (distance wp2 wp1) 15)"}; for (const auto & func : functions) { ASSERT_TRUE(problem_client->addFunction(plansys2::Function(func))); @@ -326,9 +305,10 @@ TEST(problem_expert, apply_atend_effect_test) bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} - }); - + while (!finish) { + exe.spin_some(); + } + }); domain_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); problem_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); @@ -354,14 +334,11 @@ TEST(problem_expert, apply_atend_effect_test) auto action_map = std::make_shared>(); (*action_map)["(move robot1 wp1 wp2):5"] = plansys2::ActionExecutionInfo(); - (*action_map)["(move robot1 wp1 wp2):5"].durative_action_info = - domain_client->getDurativeAction( + (*action_map)["(move robot1 wp1 wp2):5"].durative_action_info = domain_client->getDurativeAction( plansys2::get_action_name("(move robot1 wp1 wp2)"), plansys2::get_action_params("(move robot1 wp1 wp2)")); - ASSERT_NE( - (*action_map)["(move robot1 wp1 wp2):5"].durative_action_info, - nullptr); + ASSERT_NE((*action_map)["(move robot1 wp1 wp2):5"].durative_action_info, nullptr); std::string bt_xml_tree = R"( @@ -390,20 +367,15 @@ TEST(problem_expert, apply_atend_effect_test) try { std::vector predicates = { - "(robot_at robot1 wp1)", - "(charger_at wp2)", - "(connected wp1 wp2)"}; + "(robot_at robot1 wp1)", "(charger_at wp2)", "(connected wp1 wp2)"}; for (const auto & pred : predicates) { ASSERT_TRUE(problem_client->addPredicate(plansys2::Predicate(pred))); } std::vector functions = { - "(= (speed robot1) 3)", - "(= (max_range robot1) 75)", - "(= (state_of_charge robot1) 99)", - "(= (distance wp1 wp2) 15)", - "(= (distance wp2 wp1) 15)"}; + "(= (speed robot1) 3)", "(= (max_range robot1) 75)", "(= (state_of_charge robot1) 99)", + "(= (distance wp1 wp2) 15)", "(= (distance wp2 wp1) 15)"}; for (const auto & func : functions) { ASSERT_TRUE(problem_client->addFunction(plansys2::Function(func))); diff --git a/plansys2_executor/test/unit/execution_tree_test.cpp b/plansys2_executor/test/unit/execution_tree_test.cpp index 43cf3eaf..96f37746 100644 --- a/plansys2_executor/test/unit/execution_tree_test.cpp +++ b/plansys2_executor/test/unit/execution_tree_test.cpp @@ -12,30 +12,26 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include #include -#include #include +#include +#include +#include +#include #include "ament_index_cpp/get_package_share_directory.hpp" - #include "gtest/gtest.h" -#include "plansys2_domain_expert/DomainExpertNode.hpp" #include "plansys2_domain_expert/DomainExpertClient.hpp" -#include "plansys2_problem_expert/ProblemExpertNode.hpp" -#include "plansys2_problem_expert/ProblemExpertClient.hpp" -#include "plansys2_planner/PlannerNode.hpp" -#include "plansys2_planner/PlannerClient.hpp" +#include "plansys2_domain_expert/DomainExpertNode.hpp" #include "plansys2_executor/BTBuilder.hpp" - -#include "pluginlib/class_loader.hpp" +#include "plansys2_planner/PlannerClient.hpp" +#include "plansys2_planner/PlannerNode.hpp" +#include "plansys2_problem_expert/ProblemExpertClient.hpp" +#include "plansys2_problem_expert/ProblemExpertNode.hpp" #include "pluginlib/class_list_macros.hpp" - +#include "pluginlib/class_loader.hpp" #include "rclcpp/rclcpp.hpp" - TEST(executiotest_noden_tree, bt_builder_factory) { auto test_node = rclcpp::Node::make_shared("get_action_from_string"); @@ -59,9 +55,10 @@ TEST(executiotest_noden_tree, bt_builder_factory) bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} - }); - + while (!finish) { + exe.spin_some(); + } + }); domain_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); problem_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); @@ -152,16 +149,15 @@ TEST(executiotest_noden_tree, bt_builder_factory) } ASSERT_TRUE( - problem_client->setGoal( - plansys2::Goal( - "(and (car_assembled car_1) (car_assembled car_2) (car_assembled car_3))"))); + problem_client->setGoal(plansys2::Goal("(and (car_assembled car_1) (car_assembled car_2) " + "(car_assembled car_3))"))); auto plan = planner_client->getPlan(domain_client->getDomain(), problem_client->getProblem()); ASSERT_TRUE(plan); std::shared_ptr bt_builder; - pluginlib::ClassLoader bt_builder_loader("plansys2_executor", - "plansys2::BTBuilder"); + pluginlib::ClassLoader bt_builder_loader( + "plansys2_executor", "plansys2::BTBuilder"); try { bt_builder = bt_builder_loader.createSharedInstance("plansys2::SimpleBTBuilder"); } catch (pluginlib::PluginlibException & ex) { @@ -176,7 +172,6 @@ TEST(executiotest_noden_tree, bt_builder_factory) t.join(); } - TEST(executiotest_noden_tree, bt_builder_factory_2) { auto test_node = rclcpp::Node::make_shared("get_action_from_string"); @@ -200,9 +195,10 @@ TEST(executiotest_noden_tree, bt_builder_factory_2) bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} - }); - + while (!finish) { + exe.spin_some(); + } + }); domain_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); problem_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); @@ -292,18 +288,17 @@ TEST(executiotest_noden_tree, bt_builder_factory_2) ASSERT_TRUE(problem_client->addPredicate(plansys2::Predicate(pred))); } - ASSERT_TRUE( - problem_client->setGoal( - plansys2::Goal( - std::string("(and (car_assembled car_1) (piece_at body_car_2 assembly_zone)") + - std::string("(piece_at body_car_3 assembly_zone))")))); + ASSERT_TRUE(problem_client->setGoal(plansys2::Goal( + std::string("(and (car_assembled car_1) (piece_at " + "body_car_2 assembly_zone)") + + std::string("(piece_at body_car_3 assembly_zone))")))); auto plan = planner_client->getPlan(domain_client->getDomain(), problem_client->getProblem()); ASSERT_TRUE(plan); std::shared_ptr bt_builder; - pluginlib::ClassLoader bt_builder_loader("plansys2_executor", - "plansys2::BTBuilder"); + pluginlib::ClassLoader bt_builder_loader( + "plansys2_executor", "plansys2::BTBuilder"); try { bt_builder = bt_builder_loader.createSharedInstance("plansys2::SimpleBTBuilder"); } catch (pluginlib::PluginlibException & ex) { @@ -340,9 +335,10 @@ TEST(executiotest_noden_tree, bt_builder_factory_3) bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} - }); - + while (!finish) { + exe.spin_some(); + } + }); domain_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); problem_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); @@ -373,22 +369,13 @@ TEST(executiotest_noden_tree, bt_builder_factory_3) ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("wp_control", "waypoint"))); for (unsigned i = 1; i <= 4; i++) { ASSERT_TRUE( - problem_client->addInstance( - plansys2::Instance( - "wp" + std::to_string( - i), "waypoint"))); + problem_client->addInstance(plansys2::Instance("wp" + std::to_string(i), "waypoint"))); } std::vector predicates = { - "(robot_at r2d2 wp_control)", - "(charger_at wp3)", - "(connected wp_control wp1)", - "(connected wp1 wp_control)", - "(connected wp_control wp2)", - "(connected wp2 wp_control)", - "(connected wp_control wp3)", - "(connected wp3 wp_control)", - "(connected wp_control wp4)", + "(robot_at r2d2 wp_control)", "(charger_at wp3)", "(connected wp_control wp1)", + "(connected wp1 wp_control)", "(connected wp_control wp2)", "(connected wp2 wp_control)", + "(connected wp_control wp3)", "(connected wp3 wp_control)", "(connected wp_control wp4)", "(connected wp4 wp_control)"}; for (const auto & pred : predicates) { @@ -425,16 +412,15 @@ TEST(executiotest_noden_tree, bt_builder_factory_3) } ASSERT_TRUE( - problem_client->setGoal( - plansys2::Goal( - "(and (patrolled wp1) (patrolled wp2) (patrolled wp3) (patrolled wp4))"))); + problem_client->setGoal(plansys2::Goal("(and (patrolled wp1) (patrolled wp2) (patrolled wp3) " + "(patrolled wp4))"))); auto plan = planner_client->getPlan(domain_client->getDomain(), problem_client->getProblem()); ASSERT_TRUE(plan); std::shared_ptr bt_builder; - pluginlib::ClassLoader bt_builder_loader("plansys2_executor", - "plansys2::BTBuilder"); + pluginlib::ClassLoader bt_builder_loader( + "plansys2_executor", "plansys2::BTBuilder"); try { bt_builder = bt_builder_loader.createSharedInstance("plansys2::SimpleBTBuilder"); } catch (pluginlib::PluginlibException & ex) { diff --git a/plansys2_executor/test/unit/executor_test.cpp b/plansys2_executor/test/unit/executor_test.cpp index 608392fb..ac68ebbf 100644 --- a/plansys2_executor/test/unit/executor_test.cpp +++ b/plansys2_executor/test/unit/executor_test.cpp @@ -12,56 +12,47 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include -#include -#include #include +#include #include +#include +#include +#include +#include #include "ament_index_cpp/get_package_share_directory.hpp" - -#include "plansys2_domain_expert/DomainExpertNode.hpp" +#include "behaviortree_cpp_v3/behavior_tree.h" +#include "behaviortree_cpp_v3/blackboard.h" +#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp_v3/utils/shared_library.h" +#include "gtest/gtest.h" +#include "lifecycle_msgs/msg/state.hpp" #include "plansys2_domain_expert/DomainExpertClient.hpp" -#include "plansys2_problem_expert/ProblemExpertNode.hpp" -#include "plansys2_problem_expert/ProblemExpertClient.hpp" -#include "plansys2_planner/PlannerNode.hpp" -#include "plansys2_planner/PlannerClient.hpp" -#include "plansys2_executor/BTBuilder.hpp" - +#include "plansys2_domain_expert/DomainExpertNode.hpp" #include "plansys2_executor/ActionExecutor.hpp" #include "plansys2_executor/ActionExecutorClient.hpp" -#include "plansys2_executor/ExecutorNode.hpp" +#include "plansys2_executor/BTBuilder.hpp" #include "plansys2_executor/ExecutorClient.hpp" -#include "plansys2_problem_expert/Utils.hpp" - -#include "behaviortree_cpp_v3/behavior_tree.h" -#include "behaviortree_cpp_v3/bt_factory.h" -#include "behaviortree_cpp_v3/utils/shared_library.h" -#include "behaviortree_cpp_v3/blackboard.h" - +#include "plansys2_executor/ExecutorNode.hpp" +#include "plansys2_executor/behavior_tree/apply_atend_effect_node.hpp" +#include "plansys2_executor/behavior_tree/apply_atstart_effect_node.hpp" +#include "plansys2_executor/behavior_tree/check_atend_req_node.hpp" +#include "plansys2_executor/behavior_tree/check_overall_req_node.hpp" #include "plansys2_executor/behavior_tree/execute_action_node.hpp" #include "plansys2_executor/behavior_tree/wait_action_node.hpp" #include "plansys2_executor/behavior_tree/wait_atstart_req_node.hpp" -#include "plansys2_executor/behavior_tree/check_overall_req_node.hpp" -#include "plansys2_executor/behavior_tree/check_atend_req_node.hpp" -#include "plansys2_executor/behavior_tree/apply_atstart_effect_node.hpp" -#include "plansys2_executor/behavior_tree/apply_atend_effect_node.hpp" - -#include "lifecycle_msgs/msg/state.hpp" #include "plansys2_msgs/msg/action_execution_info.hpp" - -#include "pluginlib/class_loader.hpp" +#include "plansys2_planner/PlannerClient.hpp" +#include "plansys2_planner/PlannerNode.hpp" +#include "plansys2_problem_expert/ProblemExpertClient.hpp" +#include "plansys2_problem_expert/ProblemExpertNode.hpp" +#include "plansys2_problem_expert/Utils.hpp" #include "pluginlib/class_list_macros.hpp" - +#include "pluginlib/class_loader.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "gtest/gtest.h" - - TEST(executor, action_executor_api) { auto node = rclcpp_lifecycle::LifecycleNode::make_shared("test_node"); @@ -75,8 +66,7 @@ TEST(executor, action_executor_api) ASSERT_EQ(action_executor_1->get_status(), BT::NodeStatus::IDLE); } -using CallbackReturnT = - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; +using CallbackReturnT = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; using namespace std::chrono_literals; class MoveAction : public plansys2::ActionExecutorClient @@ -88,22 +78,14 @@ class MoveAction : public plansys2::ActionExecutorClient return std::make_shared(node_name, rate); } - MoveAction(const std::string & id, const std::chrono::nanoseconds & rate) - : ActionExecutorClient(id, rate), - executions_(0), - cycles_(0), - runtime_(0) + : ActionExecutorClient(id, rate), executions_(0), cycles_(0), runtime_(0) { } - void set_runtime(double runtime) - { - runtime_ = runtime; - } + void set_runtime(double runtime) { runtime_ = runtime; } - CallbackReturnT - on_activate(const rclcpp_lifecycle::State & state) + CallbackReturnT on_activate(const rclcpp_lifecycle::State & state) { std::cerr << "MoveAction::on_activate" << std::endl; counter_ = 0; @@ -121,8 +103,8 @@ class MoveAction : public plansys2::ActionExecutorClient cycles_++; auto current_time = std::chrono::high_resolution_clock::now(); - auto elapsed_time = std::chrono::duration_cast( - current_time - start_); + auto elapsed_time = + std::chrono::duration_cast(current_time - start_); if (runtime_ > 1e-5) { if (elapsed_time > std::chrono::duration(runtime_)) { @@ -158,7 +140,6 @@ class TransportAction : public plansys2::ActionExecutorClient return std::make_shared(node_name, rate); } - TransportAction(const std::string & id, const std::chrono::nanoseconds & rate) : ActionExecutorClient(id, rate) { @@ -166,8 +147,7 @@ class TransportAction : public plansys2::ActionExecutorClient cycles_ = 0; } - CallbackReturnT - on_activate(const rclcpp_lifecycle::State & state) + CallbackReturnT on_activate(const rclcpp_lifecycle::State & state) { std::cerr << "TransportAction::on_activate" << std::endl; counter_ = 0; @@ -235,16 +215,15 @@ TEST(executor, action_executor_client) std::vector history_msgs; bool confirmed = false; auto actions_sub = aux_node->create_subscription( - "actions_hub", - rclcpp::QoS(100).reliable(), [&](plansys2_msgs::msg::ActionExecution::UniquePtr msg) { - history_msgs.push_back(*msg); - }); + "actions_hub", rclcpp::QoS(100).reliable(), + [&](plansys2_msgs::msg::ActionExecution::UniquePtr msg) { history_msgs.push_back(*msg); }); bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} - }); - + while (!finish) { + exe.spin_some(); + } + }); std::string bt_xml_tree = R"( @@ -278,7 +257,6 @@ TEST(executor, action_executor_client) auto tree = factory.createTreeFromText(bt_xml_tree, blackboard); - auto status = BT::NodeStatus::RUNNING; while (status != BT::NodeStatus::SUCCESS) { status = tree.tickRoot(); @@ -293,7 +271,6 @@ TEST(executor, action_executor_client) ASSERT_EQ(transport_action_node->cycles_, 15); // ASSERT_EQ(history_msgs.size(), 64); - finish = true; t.join(); } @@ -321,9 +298,10 @@ TEST(executor, action_executor) bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} - }); - + while (!finish) { + exe.spin_some(); + } + }); domain_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); problem_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); @@ -418,10 +396,9 @@ TEST(executor, action_executor) auto plan = planner_client->getPlan(domain_client->getDomain(), problem_client->getProblem()); ASSERT_TRUE(plan); - std::shared_ptr bt_builder; - pluginlib::ClassLoader bt_builder_loader("plansys2_executor", - "plansys2::BTBuilder"); + pluginlib::ClassLoader bt_builder_loader( + "plansys2_executor", "plansys2::BTBuilder"); try { bt_builder = bt_builder_loader.createSharedInstance("plansys2::SimpleBTBuilder"); } catch (pluginlib::PluginlibException & ex) { @@ -438,10 +415,10 @@ TEST(executor, action_executor) class ExecuteActionTest : public plansys2::ExecuteAction { public: - ExecuteActionTest( - const std::string & xml_tag_name, - const BT::NodeConfiguration & conf) - : ExecuteAction(xml_tag_name, conf) {} + ExecuteActionTest(const std::string & xml_tag_name, const BT::NodeConfiguration & conf) + : ExecuteAction(xml_tag_name, conf) + { + } void halt() override { @@ -463,10 +440,10 @@ class ExecuteActionTest : public plansys2::ExecuteAction class WaitActionTest : public plansys2::WaitAction { public: - WaitActionTest( - const std::string & xml_tag_name, - const BT::NodeConfiguration & conf) - : WaitAction(xml_tag_name, conf) {} + WaitActionTest(const std::string & xml_tag_name, const BT::NodeConfiguration & conf) + : WaitAction(xml_tag_name, conf) + { + } void halt() override { @@ -487,10 +464,10 @@ class WaitActionTest : public plansys2::WaitAction class CheckOverAllReqTest : public plansys2::CheckOverAllReq { public: - CheckOverAllReqTest( - const std::string & xml_tag_name, - const BT::NodeConfiguration & conf) - : CheckOverAllReq(xml_tag_name, conf) {} + CheckOverAllReqTest(const std::string & xml_tag_name, const BT::NodeConfiguration & conf) + : CheckOverAllReq(xml_tag_name, conf) + { + } void halt() override { @@ -511,10 +488,10 @@ class CheckOverAllReqTest : public plansys2::CheckOverAllReq class WaitAtStartReqTest : public plansys2::WaitAtStartReq { public: - WaitAtStartReqTest( - const std::string & xml_tag_name, - const BT::NodeConfiguration & conf) - : WaitAtStartReq(xml_tag_name, conf) {} + WaitAtStartReqTest(const std::string & xml_tag_name, const BT::NodeConfiguration & conf) + : WaitAtStartReq(xml_tag_name, conf) + { + } void halt() override { @@ -535,10 +512,10 @@ class WaitAtStartReqTest : public plansys2::WaitAtStartReq class CheckAtEndReqTest : public plansys2::CheckAtEndReq { public: - CheckAtEndReqTest( - const std::string & xml_tag_name, - const BT::NodeConfiguration & conf) - : CheckAtEndReq(xml_tag_name, conf) {} + CheckAtEndReqTest(const std::string & xml_tag_name, const BT::NodeConfiguration & conf) + : CheckAtEndReq(xml_tag_name, conf) + { + } void halt() override { @@ -559,10 +536,10 @@ class CheckAtEndReqTest : public plansys2::CheckAtEndReq class ApplyAtStartEffectTest : public plansys2::ApplyAtStartEffect { public: - ApplyAtStartEffectTest( - const std::string & xml_tag_name, - const BT::NodeConfiguration & conf) - : ApplyAtStartEffect(xml_tag_name, conf) {} + ApplyAtStartEffectTest(const std::string & xml_tag_name, const BT::NodeConfiguration & conf) + : ApplyAtStartEffect(xml_tag_name, conf) + { + } void halt() override { @@ -583,10 +560,10 @@ class ApplyAtStartEffectTest : public plansys2::ApplyAtStartEffect class ApplyAtEndEffectTest : public plansys2::ApplyAtEndEffect { public: - ApplyAtEndEffectTest( - const std::string & xml_tag_name, - const BT::NodeConfiguration & conf) - : ApplyAtEndEffect(xml_tag_name, conf) {} + ApplyAtEndEffectTest(const std::string & xml_tag_name, const BT::NodeConfiguration & conf) + : ApplyAtEndEffect(xml_tag_name, conf) + { + } void halt() override { @@ -648,12 +625,12 @@ TEST(executor, action_real_action_1) exe.add_node(move_action_node->get_node_base_interface()); exe.add_node(test_lf_node->get_node_base_interface()); - bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} - }); - + while (!finish) { + exe.spin_some(); + } + }); domain_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); problem_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); @@ -713,11 +690,11 @@ TEST(executor, action_real_action_1) plansys2::ActionExecutionInfo(); (*action_map)["(move r2d2 steering_wheels_zone assembly_zone):0"].action_executor = plansys2::ActionExecutor::make_shared( - "(move r2d2 steering_wheels_zone assembly_zone)", test_lf_node); + "(move r2d2 steering_wheels_zone assembly_zone)", test_lf_node); (*action_map)["(move r2d2 steering_wheels_zone assembly_zone):0"].durative_action_info = domain_client->getDurativeAction( - plansys2::get_action_name("(move r2d2 steering_wheels_zone assembly_zone):0"), - plansys2::get_action_params("(move r2d2 steering_wheels_zone assembly_zone):0")); + plansys2::get_action_name("(move r2d2 steering_wheels_zone assembly_zone):0"), + plansys2::get_action_params("(move r2d2 steering_wheels_zone assembly_zone):0")); auto blackboard = BT::Blackboard::create(); @@ -769,7 +746,6 @@ TEST(executor, action_real_action_1) )"; - std::vector predicates = { "(robot_at r2d2 steering_wheels_zone)", "(robot_available r2d2)", @@ -791,9 +767,7 @@ TEST(executor, action_real_action_1) ASSERT_EQ(ApplyAtStartEffectTest::test_status, BT::NodeStatus::SUCCESS); ASSERT_FALSE(problem_client->existPredicate(plansys2::Predicate("(robot_available r2d2)"))); ASSERT_FALSE( - problem_client->existPredicate( - plansys2::Predicate( - "(robot_at r2d2 steering_wheels_zone)"))); + problem_client->existPredicate(plansys2::Predicate("(robot_at r2d2 steering_wheels_zone)"))); status = tree.tickRoot(); ASSERT_EQ(CheckOverAllReqTest::test_status, BT::NodeStatus::SUCCESS); @@ -833,9 +807,7 @@ TEST(executor, action_real_action_1) } ASSERT_FALSE( - problem_client->existPredicate( - plansys2::Predicate( - "(robot_at r2d2 assembly_zone)"))); + problem_client->existPredicate(plansys2::Predicate("(robot_at r2d2 assembly_zone)"))); ASSERT_FALSE(problem_client->existPredicate(plansys2::Predicate("(robot_available r2d2)"))); while (ExecuteActionTest::test_status != BT::NodeStatus::SUCCESS) { @@ -854,7 +826,6 @@ TEST(executor, action_real_action_1) std::cerr << e.what() << '\n'; } - ExecuteActionTest::test_status = BT::NodeStatus::IDLE; WaitActionTest::test_status = BT::NodeStatus::IDLE; CheckOverAllReqTest::test_status = BT::NodeStatus::IDLE; @@ -903,12 +874,12 @@ TEST(executor, cancel_bt_execution) exe.add_node(move_action_node->get_node_base_interface()); exe.add_node(test_lf_node->get_node_base_interface()); - bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} - }); - + while (!finish) { + exe.spin_some(); + } + }); domain_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); problem_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); @@ -967,11 +938,11 @@ TEST(executor, cancel_bt_execution) plansys2::ActionExecutionInfo(); (*action_map)["(move r2d2 steering_wheels_zone assembly_zone):0"].action_executor = plansys2::ActionExecutor::make_shared( - "(move r2d2 steering_wheels_zone assembly_zone)", test_lf_node); + "(move r2d2 steering_wheels_zone assembly_zone)", test_lf_node); (*action_map)["(move r2d2 steering_wheels_zone assembly_zone):0"].durative_action_info = domain_client->getDurativeAction( - plansys2::get_action_name("(move r2d2 steering_wheels_zone assembly_zone):0"), - plansys2::get_action_params("(move r2d2 steering_wheels_zone assembly_zone):0")); + plansys2::get_action_name("(move r2d2 steering_wheels_zone assembly_zone):0"), + plansys2::get_action_params("(move r2d2 steering_wheels_zone assembly_zone):0")); auto blackboard = BT::Blackboard::create(); @@ -1010,9 +981,7 @@ TEST(executor, cancel_bt_execution) ASSERT_EQ(ApplyAtStartEffectTest::test_status, BT::NodeStatus::SUCCESS); ASSERT_FALSE(problem_client->existPredicate(plansys2::Predicate("(robot_available r2d2)"))); ASSERT_FALSE( - problem_client->existPredicate( - plansys2::Predicate( - "(robot_at r2d2 steering_wheels_zone)"))); + problem_client->existPredicate(plansys2::Predicate("(robot_at r2d2 steering_wheels_zone)"))); status = tree.tickRoot(); ASSERT_EQ(CheckOverAllReqTest::test_status, BT::NodeStatus::SUCCESS); @@ -1070,7 +1039,7 @@ TEST(executor, cancel_bt_execution) class ExecutorNodeTest : public plansys2::ExecutorNode { public: - bool is_cancelled() {return cancel_plan_requested_;} + bool is_cancelled() { return cancel_plan_requested_; } }; TEST(executor, executor_client_execute_plan) @@ -1106,12 +1075,12 @@ TEST(executor, executor_client_execute_plan) exe.add_node(move_action_node->get_node_base_interface()); exe.add_node(test_lf_node->get_node_base_interface()); - bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} - }); - + while (!finish) { + exe.spin_some(); + } + }); domain_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); problem_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); @@ -1210,8 +1179,7 @@ TEST(executor, executor_client_execute_plan) } } - ASSERT_TRUE( - problem_client->existPredicate(plansys2::Predicate("(robot_at r2d2 body_car_zone)"))); + ASSERT_TRUE(problem_client->existPredicate(plansys2::Predicate("(robot_at r2d2 body_car_zone)"))); ASSERT_TRUE(executor_client->getResult().has_value()); result = executor_client->getResult().value(); @@ -1232,7 +1200,6 @@ class PatrolAction : public plansys2::ActionExecutorClient return std::make_shared(node_name, rate); } - PatrolAction(const std::string & id, const std::chrono::nanoseconds & rate) : ActionExecutorClient(id, rate) { @@ -1240,8 +1207,7 @@ class PatrolAction : public plansys2::ActionExecutorClient cycles_ = 0; } - CallbackReturnT - on_activate(const rclcpp_lifecycle::State & state) + CallbackReturnT on_activate(const rclcpp_lifecycle::State & state) { std::cerr << "PatrolAction::on_activate" << std::endl; counter_ = 0; @@ -1310,8 +1276,10 @@ TEST(executor, executor_client_ordered_sub_goals) bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} - }); + while (!finish) { + exe.spin_some(); + } + }); domain_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); problem_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); @@ -1349,28 +1317,17 @@ TEST(executor, executor_client_ordered_sub_goals) ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("wp2", "waypoint"))); std::vector predicates = { - "(robot_at r2d2 wp0)", - "(connected wp0 wp1)", - "(connected wp1 wp0)", - "(connected wp0 wp2)", - "(connected wp2 wp0)", - "(connected wp1 wp2)", - "(connected wp2 wp1)", + "(robot_at r2d2 wp0)", "(connected wp0 wp1)", "(connected wp1 wp0)", "(connected wp0 wp2)", + "(connected wp2 wp0)", "(connected wp1 wp2)", "(connected wp2 wp1)", }; for (const auto & pred : predicates) { ASSERT_TRUE(problem_client->addPredicate(plansys2::Predicate(pred))); } std::vector functions = { - "(= (speed r2d2) 1.0)", - "(= (max_range r2d2) 100.0)", - "(= (state_of_charge r2d2) 100.0)", - "(= (distance wp0 wp1) 5.0)", - "(= (distance wp1 wp0) 5.0)", - "(= (distance wp0 wp2) 15.0)", - "(= (distance wp2 wp0) 15.0)", - "(= (distance wp1 wp2) 5.0)", - "(= (distance wp2 wp1) 5.0)", + "(= (speed r2d2) 1.0)", "(= (max_range r2d2) 100.0)", "(= (state_of_charge r2d2) 100.0)", + "(= (distance wp0 wp1) 5.0)", "(= (distance wp1 wp0) 5.0)", "(= (distance wp0 wp2) 15.0)", + "(= (distance wp2 wp0) 15.0)", "(= (distance wp1 wp2) 5.0)", "(= (distance wp2 wp1) 5.0)", }; for (const auto & func : functions) { ASSERT_TRUE(problem_client->addFunction(plansys2::Function(func))); @@ -1409,8 +1366,7 @@ TEST(executor, executor_client_ordered_sub_goals) ASSERT_EQ(actual_sub_goals.size(), expected_sub_goals.size()); for (size_t i = 0; i < actual_sub_goals.size(); i++) { ASSERT_EQ( - parser::pddl::toString(actual_sub_goals[i]), - parser::pddl::toString(expected_sub_goals[i])); + parser::pddl::toString(actual_sub_goals[i]), parser::pddl::toString(expected_sub_goals[i])); } finish = true; @@ -1450,12 +1406,12 @@ TEST(executor, executor_client_cancel_plan) exe.add_node(move_action_node->get_node_base_interface()); exe.add_node(test_lf_node->get_node_base_interface()); - bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} - }); - + while (!finish) { + exe.spin_some(); + } + }); domain_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); problem_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); @@ -1519,11 +1475,11 @@ TEST(executor, executor_client_cancel_plan) while (rclcpp::ok() && executor_client->execute_and_check_plan()) { auto feedback = executor_client->getFeedBack(); - if (!feedback.action_execution_status.empty() && + if ( + !feedback.action_execution_status.empty() && feedback.action_execution_status[0].status == - plansys2_msgs::msg::ActionExecutionInfo::EXECUTING && - (test_node_1->now() - start).seconds() > 3) - { + plansys2_msgs::msg::ActionExecutionInfo::EXECUTING && + (test_node_1->now() - start).seconds() > 3) { executor_client->cancel_plan_execution(); break; } @@ -1542,14 +1498,12 @@ TEST(executor, executor_client_cancel_plan) } ASSERT_EQ( - move_action_node->get_current_state().id(), - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + move_action_node->get_current_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); finish = true; t.join(); } - TEST(executor, action_timeout) { auto test_node_1 = rclcpp::Node::make_shared("test_node_1"); @@ -1577,12 +1531,12 @@ TEST(executor, action_timeout) problem_node->set_parameter({"model_file", pkgpath + "/pddl/factory3.pddl"}); executor_node->set_parameter( {"default_action_bt_xml_filename", - pkgpath + "/test_behavior_trees/test_action_timeout_bt.xml"}); + pkgpath + "/test_behavior_trees/test_action_timeout_bt.xml"}); executor_node->set_parameter({"bt_builder_plugin", "SimpleBTBuilder"}); executor_node->set_parameter({"action_timeouts.actions", std::vector({"move"})}); - // have to declare because the actions vector above was not available at node creation - executor_node->declare_parameter( - "action_timeouts.move.duration_overrun_percentage", 1.0); + // have to declare because the actions vector above was not available at node + // creation + executor_node->declare_parameter("action_timeouts.move.duration_overrun_percentage", 1.0); executor_node->set_parameter({"action_timeouts.move.duration_overrun_percentage", 1.0}); rclcpp::executors::MultiThreadedExecutor exe(rclcpp::ExecutorOptions(), 8); @@ -1596,8 +1550,10 @@ TEST(executor, action_timeout) bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} - }); + while (!finish) { + exe.spin_some(); + } + }); domain_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); problem_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); @@ -1664,9 +1620,8 @@ TEST(executor, action_timeout) std::stringstream ss; ss.setf(std::ios_base::fixed, std::ios_base::floatfield); for (const auto & action_feedback : feedback.action_execution_status) { - ss << "[" << action_feedback.action << " " << std::setprecision(1) << - action_feedback.completion * 100.0 << - "%]"; + ss << "[" << action_feedback.action << " " << std::setprecision(1) + << action_feedback.completion * 100.0 << "%]"; } auto & clk = *executor_node->get_clock(); RCLCPP_WARN_THROTTLE(executor_node->get_logger(), clk, 500, "%s", ss.str().c_str()); @@ -1678,8 +1633,7 @@ TEST(executor, action_timeout) auto result = executor_client->getResult().value(); ASSERT_FALSE(result.success); ASSERT_EQ( - result.action_execution_status[0].status, - plansys2_msgs::msg::ActionExecutionInfo::CANCELLED); + result.action_execution_status[0].status, plansys2_msgs::msg::ActionExecutionInfo::CANCELLED); { rclcpp::Rate rate(10); @@ -1690,14 +1644,12 @@ TEST(executor, action_timeout) } ASSERT_EQ( - move_action_node->get_current_state().id(), - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + move_action_node->get_current_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); finish = true; t.join(); } - int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/plansys2_executor/test/unit/simple_btbuilder_tests.cpp b/plansys2_executor/test/unit/simple_btbuilder_tests.cpp index 1d6eb626..d1a083b2 100644 --- a/plansys2_executor/test/unit/simple_btbuilder_tests.cpp +++ b/plansys2_executor/test/unit/simple_btbuilder_tests.cpp @@ -12,54 +12,46 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include -#include -#include #include +#include +#include #include +#include +#include #include -#include +#include #include +#include #include "ament_index_cpp/get_package_share_directory.hpp" - -#include "plansys2_domain_expert/DomainExpertNode.hpp" +#include "behaviortree_cpp_v3/behavior_tree.h" +#include "behaviortree_cpp_v3/blackboard.h" +#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp_v3/utils/shared_library.h" +#include "gtest/gtest.h" +#include "lifecycle_msgs/msg/state.hpp" #include "plansys2_domain_expert/DomainExpertClient.hpp" -#include "plansys2_executor/bt_builder_plugins/simple_bt_builder.hpp" -#include "plansys2_problem_expert/ProblemExpertNode.hpp" -#include "plansys2_problem_expert/ProblemExpertClient.hpp" -#include "plansys2_planner/PlannerNode.hpp" -#include "plansys2_planner/PlannerClient.hpp" -#include "plansys2_problem_expert/Utils.hpp" - +#include "plansys2_domain_expert/DomainExpertNode.hpp" #include "plansys2_executor/ActionExecutor.hpp" #include "plansys2_executor/ActionExecutorClient.hpp" -#include "plansys2_executor/ExecutorNode.hpp" #include "plansys2_executor/ExecutorClient.hpp" - -#include "behaviortree_cpp_v3/behavior_tree.h" -#include "behaviortree_cpp_v3/bt_factory.h" -#include "behaviortree_cpp_v3/utils/shared_library.h" -#include "behaviortree_cpp_v3/blackboard.h" - +#include "plansys2_executor/ExecutorNode.hpp" #include "plansys2_executor/behavior_tree/execute_action_node.hpp" #include "plansys2_executor/behavior_tree/wait_action_node.hpp" - -#include "lifecycle_msgs/msg/state.hpp" - +#include "plansys2_executor/bt_builder_plugins/simple_bt_builder.hpp" +#include "plansys2_planner/PlannerClient.hpp" +#include "plansys2_planner/PlannerNode.hpp" +#include "plansys2_problem_expert/ProblemExpertClient.hpp" +#include "plansys2_problem_expert/ProblemExpertNode.hpp" +#include "plansys2_problem_expert/Utils.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "gtest/gtest.h" - class SimpleBTBuilderTest : public plansys2::SimpleBTBuilder { public: - SimpleBTBuilderTest() - : SimpleBTBuilder() {} + SimpleBTBuilderTest() : SimpleBTBuilder() {} std::string get_tree(const plansys2_msgs::msg::Plan & current_plan) { @@ -72,8 +64,7 @@ class SimpleBTBuilderTest : public plansys2::SimpleBTBuilder } bool is_action_executable( - const plansys2::ActionStamped & action, - std::vector & predicates, + const plansys2::ActionStamped & action, std::vector & predicates, std::vector & functions) const { return SimpleBTBuilder::is_action_executable(action, predicates, functions); @@ -86,30 +77,26 @@ class SimpleBTBuilderTest : public plansys2::SimpleBTBuilder std::list get_roots( std::vector & action_sequence, - std::vector & predicates, - std::vector & functions, + std::vector & predicates, std::vector & functions, int & node_counter) { return SimpleBTBuilder::get_roots(action_sequence, predicates, functions, node_counter); } plansys2::GraphNode::Ptr get_node_satisfy( - const plansys2_msgs::msg::Tree & requirement, - const plansys2::Graph::Ptr & graph, + const plansys2_msgs::msg::Tree & requirement, const plansys2::Graph::Ptr & graph, const plansys2::GraphNode::Ptr & current) { return SimpleBTBuilder::get_node_satisfy(requirement, graph, current); } plansys2::GraphNode::Ptr get_node_satisfy( - const plansys2_msgs::msg::Tree & requirement, - const plansys2::GraphNode::Ptr & node, + const plansys2_msgs::msg::Tree & requirement, const plansys2::GraphNode::Ptr & node, const plansys2::GraphNode::Ptr & current) { return SimpleBTBuilder::get_node_satisfy(requirement, node, current); } - void print_graph(const plansys2::Graph::Ptr & graph) const { SimpleBTBuilder::print_graph(graph); @@ -161,9 +148,10 @@ TEST(simple_btbuilder_tests, test_plan_1) bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} - }); - + while (!finish) { + exe.spin_some(); + } + }); domain_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); problem_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); @@ -199,18 +187,12 @@ TEST(simple_btbuilder_tests, test_plan_1) ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("chargingroom", "room"))); std::vector predicate_strings = { - "(connected entrance dinning)", - "(connected dinning entrance)", - "(connected dinning kitchen)", - "(connected kitchen dinning)", - "(connected dinning bedroom)", - "(connected bedroom dinning)", - "(connected bathroom bedroom)", - "(connected bedroom bathroom)", - "(connected chargingroom kitchen)", - "(connected kitchen chargingroom)", - "(charging_point_at chargingroom)", - "(battery_low leia)", + "(connected entrance dinning)", "(connected dinning entrance)", + "(connected dinning kitchen)", "(connected kitchen dinning)", + "(connected dinning bedroom)", "(connected bedroom dinning)", + "(connected bathroom bedroom)", "(connected bedroom bathroom)", + "(connected chargingroom kitchen)", "(connected kitchen chargingroom)", + "(charging_point_at chargingroom)", "(battery_low leia)", "(robot_at leia entrance)"}; for (const auto & pred : predicate_strings) { @@ -222,7 +204,6 @@ TEST(simple_btbuilder_tests, test_plan_1) auto plan = planner_client->getPlan(domain_client->getDomain(), problem_client->getProblem()); ASSERT_TRUE(plan); - auto predicates = problem_client->getPredicates(); auto functions = problem_client->getFunctions(); @@ -246,31 +227,13 @@ TEST(simple_btbuilder_tests, test_plan_1) ASSERT_EQ(action_0_predicates[0].parameters[1].name, "entrance"); ASSERT_TRUE(action_0_predicates[0].negate); - ASSERT_TRUE( - plansys2::check( - action_sequence[0].action->at_start_requirements, - problem_client)); - - ASSERT_FALSE( - plansys2::check( - action_sequence[1].action->at_start_requirements, - problem_client)); - ASSERT_FALSE( - plansys2::check( - action_sequence[2].action->at_start_requirements, - problem_client)); - ASSERT_FALSE( - plansys2::check( - action_sequence[3].action->at_start_requirements, - problem_client)); - ASSERT_FALSE( - plansys2::check( - action_sequence[4].action->at_start_requirements, - problem_client)); - ASSERT_FALSE( - plansys2::check( - action_sequence[5].action->at_start_requirements, - problem_client)); + ASSERT_TRUE(plansys2::check(action_sequence[0].action->at_start_requirements, problem_client)); + + ASSERT_FALSE(plansys2::check(action_sequence[1].action->at_start_requirements, problem_client)); + ASSERT_FALSE(plansys2::check(action_sequence[2].action->at_start_requirements, problem_client)); + ASSERT_FALSE(plansys2::check(action_sequence[3].action->at_start_requirements, problem_client)); + ASSERT_FALSE(plansys2::check(action_sequence[4].action->at_start_requirements, problem_client)); + ASSERT_FALSE(plansys2::check(action_sequence[5].action->at_start_requirements, problem_client)); ASSERT_TRUE(btbuilder->is_action_executable(action_sequence[0], predicates, functions)); ASSERT_FALSE(btbuilder->is_action_executable(action_sequence[1], predicates, functions)); @@ -284,96 +247,76 @@ TEST(simple_btbuilder_tests, test_plan_1) predicates.begin(), predicates.end(), std::bind( &parser::pddl::checkNodeEquality, std::placeholders::_1, - parser::pddl::fromStringPredicate("(robot_at leia entrance)"))), predicates.end()); + parser::pddl::fromStringPredicate("(robot_at leia entrance)"))), + predicates.end()); ASSERT_EQ( std::find_if( predicates.begin(), predicates.end(), std::bind( &parser::pddl::checkNodeEquality, std::placeholders::_1, - parser::pddl::fromStringPredicate("(robot_at leia chargingroom)"))), predicates.end()); + parser::pddl::fromStringPredicate("(robot_at leia chargingroom)"))), + predicates.end()); - plansys2::apply( - action_sequence[0].action->at_start_effects, - predicates, functions); - plansys2::apply( - action_sequence[0].action->at_end_effects, - predicates, functions); + plansys2::apply(action_sequence[0].action->at_start_effects, predicates, functions); + plansys2::apply(action_sequence[0].action->at_end_effects, predicates, functions); ASSERT_EQ( std::find_if( predicates.begin(), predicates.end(), std::bind( &parser::pddl::checkNodeEquality, std::placeholders::_1, - parser::pddl::fromStringPredicate("(robot_at leia entrance)"))), predicates.end()); + parser::pddl::fromStringPredicate("(robot_at leia entrance)"))), + predicates.end()); ASSERT_NE( std::find_if( predicates.begin(), predicates.end(), std::bind( &parser::pddl::checkNodeEquality, std::placeholders::_1, - parser::pddl::fromStringPredicate("(robot_at leia chargingroom)"))), predicates.end()); + parser::pddl::fromStringPredicate("(robot_at leia chargingroom)"))), + predicates.end()); ASSERT_TRUE(btbuilder->is_action_executable(action_sequence[1], predicates, functions)); ASSERT_FALSE(btbuilder->is_action_executable(action_sequence[2], predicates, functions)); ASSERT_FALSE(btbuilder->is_action_executable(action_sequence[3], predicates, functions)); ASSERT_FALSE(btbuilder->is_action_executable(action_sequence[4], predicates, functions)); ASSERT_FALSE(btbuilder->is_action_executable(action_sequence[5], predicates, functions)); - plansys2::apply( - action_sequence[1].action->at_start_effects, - predicates, functions); - plansys2::apply( - action_sequence[1].action->at_end_effects, - predicates, functions); + plansys2::apply(action_sequence[1].action->at_start_effects, predicates, functions); + plansys2::apply(action_sequence[1].action->at_end_effects, predicates, functions); ASSERT_TRUE(btbuilder->is_action_executable(action_sequence[2], predicates, functions)); ASSERT_FALSE(btbuilder->is_action_executable(action_sequence[3], predicates, functions)); ASSERT_FALSE(btbuilder->is_action_executable(action_sequence[4], predicates, functions)); ASSERT_FALSE(btbuilder->is_action_executable(action_sequence[5], predicates, functions)); - plansys2::apply( - action_sequence[2].action->at_start_effects, - predicates, functions); - plansys2::apply( - action_sequence[2].action->at_end_effects, - predicates, functions); + plansys2::apply(action_sequence[2].action->at_start_effects, predicates, functions); + plansys2::apply(action_sequence[2].action->at_end_effects, predicates, functions); ASSERT_TRUE(btbuilder->is_action_executable(action_sequence[3], predicates, functions)); ASSERT_FALSE(btbuilder->is_action_executable(action_sequence[4], predicates, functions)); ASSERT_FALSE(btbuilder->is_action_executable(action_sequence[5], predicates, functions)); - plansys2::apply( - action_sequence[3].action->at_start_effects, - predicates, functions); - plansys2::apply( - action_sequence[3].action->at_end_effects, - predicates, functions); + plansys2::apply(action_sequence[3].action->at_start_effects, predicates, functions); + plansys2::apply(action_sequence[3].action->at_end_effects, predicates, functions); ASSERT_TRUE(btbuilder->is_action_executable(action_sequence[4], predicates, functions)); ASSERT_FALSE(btbuilder->is_action_executable(action_sequence[5], predicates, functions)); - plansys2::apply( - action_sequence[4].action->at_start_effects, - predicates, functions); - plansys2::apply( - action_sequence[4].action->at_end_effects, - predicates, functions); + plansys2::apply(action_sequence[4].action->at_start_effects, predicates, functions); + plansys2::apply(action_sequence[4].action->at_end_effects, predicates, functions); ASSERT_TRUE(btbuilder->is_action_executable(action_sequence[5], predicates, functions)); - plansys2::apply( - action_sequence[5].action->at_start_effects, - predicates, functions); - plansys2::apply( - action_sequence[5].action->at_end_effects, - predicates, functions); + plansys2::apply(action_sequence[5].action->at_start_effects, predicates, functions); + plansys2::apply(action_sequence[5].action->at_end_effects, predicates, functions); ASSERT_NE( std::find_if( predicates.begin(), predicates.end(), std::bind( &parser::pddl::checkNodeEquality, std::placeholders::_1, - parser::pddl::fromStringPredicate("(robot_at leia bathroom)"))), predicates.end()); + parser::pddl::fromStringPredicate("(robot_at leia bathroom)"))), + predicates.end()); finish = true; t.join(); } - TEST(simple_btbuilder_tests, test_plan_2) { auto test_node = rclcpp::Node::make_shared("test_plan_2"); @@ -400,9 +343,10 @@ TEST(simple_btbuilder_tests, test_plan_2) bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} - }); - + while (!finish) { + exe.spin_some(); + } + }); domain_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); problem_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); @@ -489,11 +433,8 @@ TEST(simple_btbuilder_tests, test_plan_2) ASSERT_TRUE(problem_client->addPredicate(plansys2::Predicate(pred))); } - - ASSERT_TRUE( - problem_client->setGoal( - plansys2::Goal( - "(and(car_assembled car_1)(car_assembled car_2)(car_assembled car_3))"))); + ASSERT_TRUE(problem_client->setGoal( + plansys2::Goal("(and(car_assembled car_1)(car_assembled car_2)(car_assembled car_3))"))); auto plan = planner_client->getPlan(domain_client->getDomain(), problem_client->getProblem()); ASSERT_TRUE(plan); @@ -510,9 +451,7 @@ TEST(simple_btbuilder_tests, test_plan_2) btbuilder->remove_existing_requirements(check_predicates, predicates, functions); ASSERT_EQ(check_predicates.size(), 1); - ASSERT_EQ( - parser::pddl::toString(check_predicates.front()), "(is_assembly_zone body_car_zone)"); - + ASSERT_EQ(parser::pddl::toString(check_predicates.front()), "(is_assembly_zone body_car_zone)"); ASSERT_EQ(problem_client->getPredicates().size(), predicates.size()); @@ -525,12 +464,8 @@ TEST(simple_btbuilder_tests, test_plan_2) ASSERT_EQ(roots.size(), 3u); // Apply roots actions for (auto & action_node : roots) { - plansys2::apply( - action_node->action.action->at_start_effects, - predicates, functions); - plansys2::apply( - action_node->action.action->at_end_effects, - predicates, functions); + plansys2::apply(action_node->action.action->at_start_effects, predicates, functions); + plansys2::apply(action_node->action.action->at_end_effects, predicates, functions); } ASSERT_NE( @@ -538,25 +473,27 @@ TEST(simple_btbuilder_tests, test_plan_2) predicates.begin(), predicates.end(), std::bind( &parser::pddl::checkNodeEquality, std::placeholders::_1, - parser::pddl::fromStringPredicate("(robot_at robot1 body_car_zone)"))), predicates.end()); + parser::pddl::fromStringPredicate("(robot_at robot1 body_car_zone)"))), + predicates.end()); ASSERT_NE( std::find_if( predicates.begin(), predicates.end(), std::bind( &parser::pddl::checkNodeEquality, std::placeholders::_1, - parser::pddl::fromStringPredicate("(robot_at robot2 steering_wheels_zone)"))), + parser::pddl::fromStringPredicate("(robot_at robot2 " + "steering_wheels_zone)"))), predicates.end()); ASSERT_NE( std::find_if( predicates.begin(), predicates.end(), std::bind( &parser::pddl::checkNodeEquality, std::placeholders::_1, - parser::pddl::fromStringPredicate("(robot_at robot3 wheels_zone)"))), predicates.end()); + parser::pddl::fromStringPredicate("(robot_at robot3 wheels_zone)"))), + predicates.end()); tree.nodes.clear(); parser::pddl::fromString( - tree, "(robot_at robot1 body_car_zone)", false, - plansys2_msgs::msg::Node::AND); + tree, "(robot_at robot1 body_car_zone)", false, plansys2_msgs::msg::Node::AND); auto node_satisfy_1 = btbuilder->get_node_satisfy(tree, *roots.begin(), nullptr); ASSERT_NE(node_satisfy_1, nullptr); ASSERT_EQ(node_satisfy_1->action.action->name, "move"); @@ -606,9 +543,10 @@ TEST(simple_btbuilder_tests, test_plan_3) bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} - }); - + while (!finish) { + exe.spin_some(); + } + }); domain_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); problem_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); @@ -641,23 +579,15 @@ TEST(simple_btbuilder_tests, test_plan_3) ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("ro3", "room"))); std::vector predicates = { - "(connected ro1 ro2)", - "(connected ro2 ro1)", - "(connected ro1 ro3)", - "(connected ro3 ro1)", - "(connected ro2 ro3)", - "(connected ro3 ro2)", - "(robot_at leia ro1)", - "(battery_full leia)"}; + "(connected ro1 ro2)", "(connected ro2 ro1)", "(connected ro1 ro3)", "(connected ro3 ro1)", + "(connected ro2 ro3)", "(connected ro3 ro2)", "(robot_at leia ro1)", "(battery_full leia)"}; for (const auto & pred : predicates) { ASSERT_TRUE(problem_client->addPredicate(plansys2::Predicate(pred))); } - ASSERT_TRUE( - problem_client->setGoal( - plansys2::Goal( - "(and (patrolled ro1) (patrolled ro2) (patrolled ro3))"))); + ASSERT_TRUE(problem_client->setGoal( + plansys2::Goal("(and (patrolled ro1) (patrolled ro2) (patrolled ro3))"))); auto plan = planner_client->getPlan(domain_client->getDomain(), problem_client->getProblem()); ASSERT_TRUE(plan); @@ -666,7 +596,6 @@ TEST(simple_btbuilder_tests, test_plan_3) std::cerr << bt << std::endl; - finish = true; t.join(); } @@ -697,9 +626,10 @@ TEST(simple_btbuilder_tests, test_plan_4) bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} - }); - + while (!finish) { + exe.spin_some(); + } + }); domain_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); problem_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); @@ -762,17 +692,14 @@ TEST(simple_btbuilder_tests, test_plan_4) "(robot_at r2d2 cooking_zone)", "(battery_full r2d2)", "(robot_at c3po cooking_zone)", - "(battery_full c3po)" - }; + "(battery_full c3po)"}; for (const auto & pred : predicates) { ASSERT_TRUE(problem_client->addPredicate(plansys2::Predicate(pred))); } ASSERT_TRUE( - problem_client->setGoal( - plansys2::Goal( - "(and (dish_prepared cake)(dish_prepared omelette))"))); + problem_client->setGoal(plansys2::Goal("(and (dish_prepared cake)(dish_prepared omelette))"))); auto plan = planner_client->getPlan(domain_client->getDomain(), problem_client->getProblem()); ASSERT_TRUE(plan); @@ -814,8 +741,10 @@ TEST(simple_btbuilder_tests, test_plan_5) bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} - }); + while (!finish) { + exe.spin_some(); + } + }); domain_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); problem_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); @@ -843,9 +772,8 @@ TEST(simple_btbuilder_tests, test_plan_5) } std::ifstream problem_ifs(pkgpath + "/pddl/road_trip_problem.pddl"); - std::string problem_str(( - std::istreambuf_iterator(problem_ifs)), - std::istreambuf_iterator()); + std::string problem_str( + (std::istreambuf_iterator(problem_ifs)), std::istreambuf_iterator()); ASSERT_TRUE(problem_client->addProblem(problem_str)); auto plan = planner_client->getPlan(domain_client->getDomain(), problem_client->getProblem()); @@ -926,8 +854,10 @@ TEST(simple_btbuilder_tests, test_plan_6) bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} - }); + while (!finish) { + exe.spin_some(); + } + }); domain_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); problem_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); @@ -955,9 +885,8 @@ TEST(simple_btbuilder_tests, test_plan_6) } std::ifstream problem_ifs(pkgpath + "/pddl/elevator_problem.pddl"); - std::string problem_str(( - std::istreambuf_iterator(problem_ifs)), - std::istreambuf_iterator()); + std::string problem_str( + (std::istreambuf_iterator(problem_ifs)), std::istreambuf_iterator()); ASSERT_TRUE(problem_client->addProblem(problem_str)); auto plan = planner_client->getPlan(domain_client->getDomain(), problem_client->getProblem()); @@ -1012,7 +941,6 @@ TEST(simple_btbuilder_tests, test_plan_6) t.join(); } - int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/plansys2_lifecycle_manager/CHANGELOG.rst b/plansys2_lifecycle_manager/CHANGELOG.rst index 9a03a256..f9667030 100644 --- a/plansys2_lifecycle_manager/CHANGELOG.rst +++ b/plansys2_lifecycle_manager/CHANGELOG.rst @@ -119,4 +119,4 @@ Changelog for package plansys2_lifecycle_manager Signed-off-by: Francisco Martin Rico * Bringup and lifecycle manager Signed-off-by: Francisco Martin Rico -* Contributors: Francisco Martín Rico \ No newline at end of file +* Contributors: Francisco Martín Rico diff --git a/plansys2_lifecycle_manager/CMakeLists.txt b/plansys2_lifecycle_manager/CMakeLists.txt index 772639bf..fb08a46b 100644 --- a/plansys2_lifecycle_manager/CMakeLists.txt +++ b/plansys2_lifecycle_manager/CMakeLists.txt @@ -40,6 +40,7 @@ install(TARGETS if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) + set(ament_cmake_uncrustify_FOUND TRUE) ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest REQUIRED) @@ -50,4 +51,4 @@ ament_export_include_directories(include) ament_export_libraries(${PROJECT_NAME}) ament_export_dependencies(${dependencies}) -ament_package() \ No newline at end of file +ament_package() diff --git a/plansys2_lifecycle_manager/include/plansys2_lifecycle_manager/lifecycle_manager.hpp b/plansys2_lifecycle_manager/include/plansys2_lifecycle_manager/lifecycle_manager.hpp index 5c8760ae..eb8239b2 100644 --- a/plansys2_lifecycle_manager/include/plansys2_lifecycle_manager/lifecycle_manager.hpp +++ b/plansys2_lifecycle_manager/include/plansys2_lifecycle_manager/lifecycle_manager.hpp @@ -16,27 +16,21 @@ #define PLANSYS2_LIFECYCLE_MANAGER__LIFECYCLE_MANAGER_HPP_ #include +#include #include #include -#include #include "lifecycle_msgs/msg/state.hpp" #include "lifecycle_msgs/msg/transition.hpp" #include "lifecycle_msgs/srv/change_state.hpp" #include "lifecycle_msgs/srv/get_state.hpp" - #include "rclcpp/rclcpp.hpp" - #include "rcutils/logging_macros.h" namespace plansys2 { - -template -std::future_status -wait_for_result( - FutureT & future, - WaitTimeT time_to_wait) +template +std::future_status wait_for_result(FutureT & future, WaitTimeT time_to_wait) { auto end = std::chrono::steady_clock::now() + time_to_wait; std::chrono::milliseconds wait_period(100); @@ -44,7 +38,9 @@ wait_for_result( do { auto now = std::chrono::steady_clock::now(); auto time_left = end - now; - if (time_left <= std::chrono::seconds(0)) {break;} + if (time_left <= std::chrono::seconds(0)) { + break; + } status = future.wait_for((time_left < wait_period) ? time_left : wait_period); } while (rclcpp::ok() && status != std::future_status::ready); return status; @@ -58,8 +54,7 @@ class LifecycleServiceClient : public rclcpp::Node void init(); unsigned int get_state(std::chrono::seconds time_out = std::chrono::seconds(3)); bool change_state( - std::uint8_t transition, - std::chrono::seconds time_out = std::chrono::seconds(3)); + std::uint8_t transition, std::chrono::seconds time_out = std::chrono::seconds(3)); private: std::shared_ptr> client_get_state_; @@ -68,8 +63,7 @@ class LifecycleServiceClient : public rclcpp::Node std::string managed_node_; }; -bool -startup_function( +bool startup_function( std::map> & manager_nodes, std::chrono::seconds timeout); diff --git a/plansys2_lifecycle_manager/package.xml b/plansys2_lifecycle_manager/package.xml index 6cf5e2a7..832f6e35 100644 --- a/plansys2_lifecycle_manager/package.xml +++ b/plansys2_lifecycle_manager/package.xml @@ -3,11 +3,11 @@ plansys2_lifecycle_manager 2.0.8 - + A controller/manager for the lifecycle nodes of the ROS2 Planning System Francisco Martin Rico - + Apache License, Version 2.0 ament_cmake @@ -15,7 +15,7 @@ rclcpp rclcpp_lifecycle lifecycle_msgs - + ament_lint_common ament_lint_auto ament_cmake_gtest diff --git a/plansys2_lifecycle_manager/src/lifecycle_manager_node.cpp b/plansys2_lifecycle_manager/src/lifecycle_manager_node.cpp index f95f8cca..bb8542e4 100644 --- a/plansys2_lifecycle_manager/src/lifecycle_manager_node.cpp +++ b/plansys2_lifecycle_manager/src/lifecycle_manager_node.cpp @@ -13,13 +13,12 @@ // limitations under the License. #include +#include #include #include -#include - -#include "rclcpp/rclcpp.hpp" #include "plansys2_lifecycle_manager/lifecycle_manager.hpp" +#include "rclcpp/rclcpp.hpp" int main(int argc, char ** argv) { @@ -28,14 +27,14 @@ int main(int argc, char ** argv) rclcpp::init(argc, argv); std::map> manager_nodes; - manager_nodes["domain_expert"] = std::make_shared( - "domain_expert_lc_mngr", "domain_expert"); - manager_nodes["problem_expert"] = std::make_shared( - "problem_expert_lc_mngr", "problem_expert"); - manager_nodes["planner"] = std::make_shared( - "planner_lc_mngr", "planner"); - manager_nodes["executor"] = std::make_shared( - "executor_lc_mngr", "executor"); + manager_nodes["domain_expert"] = + std::make_shared("domain_expert_lc_mngr", "domain_expert"); + manager_nodes["problem_expert"] = + std::make_shared("problem_expert_lc_mngr", "problem_expert"); + manager_nodes["planner"] = + std::make_shared("planner_lc_mngr", "planner"); + manager_nodes["executor"] = + std::make_shared("executor_lc_mngr", "executor"); rclcpp::executors::SingleThreadedExecutor exe; for (auto & manager_node : manager_nodes) { @@ -49,9 +48,7 @@ int main(int argc, char ** argv) exe.spin_until_future_complete(startup_future); if (!startup_future.get()) { - RCLCPP_ERROR( - rclcpp::get_logger("plansys2_lifecycle_manager"), - "Failed to start plansys2!"); + RCLCPP_ERROR(rclcpp::get_logger("plansys2_lifecycle_manager"), "Failed to start plansys2!"); rclcpp::shutdown(); return -1; } diff --git a/plansys2_lifecycle_manager/src/plansys2_lifecycle_manager/lifecycle_manager.cpp b/plansys2_lifecycle_manager/src/plansys2_lifecycle_manager/lifecycle_manager.cpp index 0aa01294..78d66b39 100644 --- a/plansys2_lifecycle_manager/src/plansys2_lifecycle_manager/lifecycle_manager.cpp +++ b/plansys2_lifecycle_manager/src/plansys2_lifecycle_manager/lifecycle_manager.cpp @@ -12,52 +12,45 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "plansys2_lifecycle_manager/lifecycle_manager.hpp" + #include +#include #include #include -#include #include "lifecycle_msgs/msg/state.hpp" #include "lifecycle_msgs/msg/transition.hpp" #include "lifecycle_msgs/srv/change_state.hpp" #include "lifecycle_msgs/srv/get_state.hpp" - #include "rclcpp/rclcpp.hpp" #include "rcutils/logging_macros.h" -#include "plansys2_lifecycle_manager/lifecycle_manager.hpp" - namespace plansys2 { - LifecycleServiceClient::LifecycleServiceClient( const std::string & node_name, const std::string & managed_node) : Node(node_name), managed_node_(managed_node) -{} +{ +} -void -LifecycleServiceClient::init() +void LifecycleServiceClient::init() { std::string get_state_service_name = managed_node_ + "/get_state"; std::string change_state_service_name = managed_node_ + "/change_state"; RCLCPP_INFO(get_logger(), "Creating client for service [%s]", get_state_service_name.c_str()); - RCLCPP_INFO( - get_logger(), "Creating client for service [%s]", - change_state_service_name.c_str()); + RCLCPP_INFO(get_logger(), "Creating client for service [%s]", change_state_service_name.c_str()); client_get_state_ = this->create_client(get_state_service_name); - client_change_state_ = this->create_client( - change_state_service_name); + client_change_state_ = + this->create_client(change_state_service_name); } -unsigned int -LifecycleServiceClient::get_state(std::chrono::seconds time_out) +unsigned int LifecycleServiceClient::get_state(std::chrono::seconds time_out) { auto request = std::make_shared(); if (!client_get_state_->wait_for_service(time_out)) { RCLCPP_ERROR( - get_logger(), - "Service %s is not available.", - client_get_state_->get_service_name()); + get_logger(), "Service %s is not available.", client_get_state_->get_service_name()); return lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN; } // We send the service request for asking the current @@ -74,29 +67,25 @@ LifecycleServiceClient::get_state(std::chrono::seconds time_out) managed_node_.c_str()); return lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN; } - // We have an succesful answer. So let's print the current state. + // We have an successful answer. So let's print the current state. if (state != nullptr) { RCLCPP_INFO( - get_logger(), "Node %s has current state %s.", - get_name(), state->current_state.label.c_str()); + get_logger(), "Node %s has current state %s.", get_name(), + state->current_state.label.c_str()); return state->current_state.id; } else { - RCLCPP_ERROR( - get_logger(), "Failed to get current state for node %s", managed_node_.c_str()); + RCLCPP_ERROR(get_logger(), "Failed to get current state for node %s", managed_node_.c_str()); return lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN; } } -bool -LifecycleServiceClient::change_state(std::uint8_t transition, std::chrono::seconds time_out) +bool LifecycleServiceClient::change_state(std::uint8_t transition, std::chrono::seconds time_out) { auto request = std::make_shared(); request->transition.id = transition; if (!client_change_state_->wait_for_service(time_out)) { RCLCPP_ERROR( - get_logger(), - "Service %s is not available.", - client_change_state_->get_service_name()); + get_logger(), "Service %s is not available.", client_change_state_->get_service_name()); return false; } // We send the request with the transition we want to invoke. @@ -122,23 +111,19 @@ LifecycleServiceClient::change_state(std::uint8_t transition, std::chrono::secon } } -bool -startup_function( +bool startup_function( std::map> & manager_nodes, std::chrono::seconds timeout) { // configure domain_expert { if (!manager_nodes["domain_expert"]->change_state( - lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE, - timeout)) - { + lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE, timeout)) { return false; } while (manager_nodes["domain_expert"]->get_state() != - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) - { + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { std::cerr << "Waiting for inactive state for domain_expert" << std::endl; } } @@ -146,15 +131,12 @@ startup_function( // configure problem_expert { if (!manager_nodes["problem_expert"]->change_state( - lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE, - timeout)) - { + lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE, timeout)) { return false; } while (manager_nodes["problem_expert"]->get_state() != - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) - { + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { std::cerr << "Waiting for inactive state for problem_expert" << std::endl; } } @@ -162,15 +144,12 @@ startup_function( // configure planner { if (!manager_nodes["planner"]->change_state( - lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE, - timeout)) - { + lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE, timeout)) { return false; } while (manager_nodes["planner"]->get_state() != - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) - { + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { std::cerr << "Waiting for inactive state for planner" << std::endl; } } @@ -178,15 +157,12 @@ startup_function( // configure executor { if (!manager_nodes["executor"]->change_state( - lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE, - timeout)) - { + lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE, timeout)) { return false; } while (manager_nodes["executor"]->get_state() != - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) - { + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { std::cerr << "Waiting for inactive state for planner" << std::endl; } } @@ -197,27 +173,19 @@ startup_function( return false; } if (!manager_nodes["domain_expert"]->change_state( - lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE, - timeout)) - { + lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE, timeout)) { return false; } if (!manager_nodes["problem_expert"]->change_state( - lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE, - timeout)) - { + lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE, timeout)) { return false; } if (!manager_nodes["planner"]->change_state( - lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE, - timeout)) - { + lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE, timeout)) { return false; } if (!manager_nodes["executor"]->change_state( - lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE, - timeout)) - { + lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE, timeout)) { return false; } if (!manager_nodes["domain_expert"]->get_state()) { diff --git a/plansys2_lifecycle_manager/test/lf_manager_test.cpp b/plansys2_lifecycle_manager/test/lf_manager_test.cpp index 598a2ff6..a8cd626c 100644 --- a/plansys2_lifecycle_manager/test/lf_manager_test.cpp +++ b/plansys2_lifecycle_manager/test/lf_manager_test.cpp @@ -12,20 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include -#include #include "gtest/gtest.h" - #include "lifecycle_msgs/msg/state.hpp" #include "lifecycle_msgs/msg/transition.hpp" - +#include "plansys2_lifecycle_manager/lifecycle_manager.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "plansys2_lifecycle_manager/lifecycle_manager.hpp" - using namespace std::chrono_literals; TEST(lifecycle_manager, lf_client) @@ -39,17 +36,19 @@ TEST(lifecycle_manager, lf_client) bool finish = false; std::thread t([&]() { - while (!finish) {exe->spin_some();} - }); + while (!finish) { + exe->spin_some(); + } + }); client_node->init(); auto start = test_node->now(); - while ((test_node->now() - start).seconds() < 1.0) {} + while ((test_node->now() - start).seconds() < 1.0) { + } ASSERT_EQ( - test_node->get_current_state().id(), - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + test_node->get_current_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); ASSERT_EQ(client_node->get_state(1s), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); ASSERT_FALSE(client_node->change_state(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE)); @@ -75,27 +74,23 @@ TEST(lifecycle_manager, lf_startup) auto ex_node = rclcpp_lifecycle::LifecycleNode::make_shared("executor"); ASSERT_EQ( - de_node->get_current_state().id(), - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + de_node->get_current_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); ASSERT_EQ( - pe_node->get_current_state().id(), - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + pe_node->get_current_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); ASSERT_EQ( - pl_node->get_current_state().id(), - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + pl_node->get_current_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); ASSERT_EQ( - ex_node->get_current_state().id(), - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ex_node->get_current_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); std::map> manager_nodes; - manager_nodes["domain_expert"] = std::make_shared( - "domain_expert_lc_mngr", "domain_expert"); - manager_nodes["problem_expert"] = std::make_shared( - "domain_expert_lc_mngr", "problem_expert"); - manager_nodes["planner"] = std::make_shared( - "domain_expert_lc_mngr", "planner"); - manager_nodes["executor"] = std::make_shared( - "domain_expert_lc_mngr", "executor"); + manager_nodes["domain_expert"] = + std::make_shared("domain_expert_lc_mngr", "domain_expert"); + manager_nodes["problem_expert"] = + std::make_shared("domain_expert_lc_mngr", "problem_expert"); + manager_nodes["planner"] = + std::make_shared("domain_expert_lc_mngr", "planner"); + manager_nodes["executor"] = + std::make_shared("domain_expert_lc_mngr", "executor"); rclcpp::executors::SingleThreadedExecutor exe; for (auto & manager_node : manager_nodes) { @@ -109,12 +104,15 @@ TEST(lifecycle_manager, lf_startup) exe.add_node(ex_node->get_node_base_interface()); auto start = de_node->now(); - while ((de_node->now() - start).seconds() < 1.0) {} + while ((de_node->now() - start).seconds() < 1.0) { + } bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} - }); + while (!finish) { + exe.spin_some(); + } + }); std::shared_future startup_future = std::async( std::launch::async, @@ -122,24 +120,17 @@ TEST(lifecycle_manager, lf_startup) startup_future.wait(); - ASSERT_EQ( - de_node->get_current_state().id(), - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + ASSERT_EQ(de_node->get_current_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); - ASSERT_EQ( - pe_node->get_current_state().id(), - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + ASSERT_EQ(pe_node->get_current_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); - ASSERT_EQ( - pl_node->get_current_state().id(), - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + ASSERT_EQ(pl_node->get_current_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); - ASSERT_EQ( - ex_node->get_current_state().id(), - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + ASSERT_EQ(ex_node->get_current_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); start = de_node->now(); - while ((de_node->now() - start).seconds() < 1.0) {} + while ((de_node->now() - start).seconds() < 1.0) { + } finish = true; t.join(); diff --git a/plansys2_msgs/CHANGELOG.rst b/plansys2_msgs/CHANGELOG.rst index 7474cd98..7f1a5729 100644 --- a/plansys2_msgs/CHANGELOG.rst +++ b/plansys2_msgs/CHANGELOG.rst @@ -141,4 +141,4 @@ Changelog for package plansys2_msgs Signed-off-by: Francisco Martin Rico * First version of domain expert Signed-off-by: Francisco Martin Rico -* Contributors: Francisco Martín Rico \ No newline at end of file +* Contributors: Francisco Martín Rico diff --git a/plansys2_msgs/README.md b/plansys2_msgs/README.md index 74f3ee82..97204ef3 100644 --- a/plansys2_msgs/README.md +++ b/plansys2_msgs/README.md @@ -36,10 +36,10 @@ The contents of `plansys2_msgs/Node` contains the data relevant to the node as w uint8 node_type uint8 expression_type uint8 modifier_type - + uint32 node_id uint32[] children - + string name plansys2_msgs/Param[] parameters float64 value @@ -107,7 +107,7 @@ have a `negate` value opposite that of the `NOT` node's value. [`plansys2_msgs::msg::ActionExecution`](../plansys2_msgs/msg/ActionExecution.msg) * Used to establish a communication protocol between the action executors (implemented as behavior -trees) and the action executor clients (user provided lifecyle nodes that perform the actions). +trees) and the action executor clients (user provided lifecycle nodes that perform the actions). [`plansys2_msgs::msg::ActionExecutionInfo`](../plansys2_msgs/msg/ActionExecutionInfo.msg) diff --git a/plansys2_msgs/package.xml b/plansys2_msgs/package.xml index dbb4ce95..211f7df3 100644 --- a/plansys2_msgs/package.xml +++ b/plansys2_msgs/package.xml @@ -3,11 +3,11 @@ plansys2_msgs 2.0.8 - + Messages and service files for the ROS2 Planning System Francisco Martin Rico - + Apache License, Version 2.0 ament_cmake diff --git a/plansys2_msgs/srv/ClearProblemKnowledge.srv b/plansys2_msgs/srv/ClearProblemKnowledge.srv index 91a0996d..6ffd43d9 100644 --- a/plansys2_msgs/srv/ClearProblemKnowledge.srv +++ b/plansys2_msgs/srv/ClearProblemKnowledge.srv @@ -1,4 +1,4 @@ std_msgs/Empty request --- bool success -string error_info \ No newline at end of file +string error_info diff --git a/plansys2_msgs/srv/RemoveProblemGoal.srv b/plansys2_msgs/srv/RemoveProblemGoal.srv index 91a0996d..6ffd43d9 100644 --- a/plansys2_msgs/srv/RemoveProblemGoal.srv +++ b/plansys2_msgs/srv/RemoveProblemGoal.srv @@ -1,4 +1,4 @@ std_msgs/Empty request --- bool success -string error_info \ No newline at end of file +string error_info diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/Action.h b/plansys2_pddl_parser/include/plansys2_pddl_parser/Action.h index ea58a3b2..5daeea00 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/Action.h +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/Action.h @@ -3,79 +3,80 @@ #include "plansys2_msgs/msg/node.hpp" #include "plansys2_msgs/msg/tree.hpp" - #include "plansys2_pddl_parser/Ground.h" -namespace parser { namespace pddl { - -class Action : public ParamCond { - +namespace parser +{ +namespace pddl +{ +class Action : public ParamCond +{ public: + Condition *pre, *eff; - Condition *pre, *eff; + Action(const std::string & s) : ParamCond(s), pre(0), eff(0) {} - Action( const std::string & s ) - : ParamCond( s ), pre( 0 ), eff( 0 ) {} + Action(ParamCond * c) : ParamCond(c), pre(0), eff(0) {} - Action( ParamCond * c ) - : ParamCond( c ), pre( 0 ), eff( 0 ) {} + Action(const Action * a, Domain & d) : ParamCond(a), pre(0), eff(0) + { + if (a->pre) pre = a->pre->copy(d); + if (a->eff) eff = a->eff->copy(d); + } - Action( const Action * a, Domain & d ) - : ParamCond( a ), pre( 0 ), eff( 0 ) { - if ( a->pre ) pre = a->pre->copy( d ); - if ( a->eff ) eff = a->eff->copy( d ); - } + virtual ~Action() + { + if (pre) delete pre; + if (eff) delete eff; + } - virtual ~Action() { - if ( pre ) delete pre; - if ( eff ) delete eff; - } + void print(std::ostream & s) const + { + s << name << params << "\n"; + s << "Pre: " << pre; + if (eff) s << "Eff: " << eff; + } - void print( std::ostream & s ) const { - s << name << params << "\n"; - s << "Pre: " << pre; - if ( eff ) s << "Eff: " << eff; - } + virtual double duration() { return 1; } - virtual double duration() { - return 1; - } + void PDDLPrint( + std::ostream & s, unsigned indent, const TokenStruct & ts, + const Domain & d) const override; - void PDDLPrint( std::ostream & s, unsigned indent, const TokenStruct< std::string > & ts, const Domain & d ) const override; + plansys2_msgs::msg::Node::SharedPtr getTree( + plansys2_msgs::msg::Tree & tree, const Domain & d, + const std::vector & replace = {}) const override; - plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace = {} ) const override; + void parseConditions(Stringreader & f, TokenStruct & ts, Domain & d); - void parseConditions( Stringreader & f, TokenStruct< std::string > & ts, Domain & d ); + void parse(Stringreader & f, TokenStruct & ts, Domain & d); - void parse( Stringreader & f, TokenStruct< std::string > & ts, Domain & d ); + void addParams(int m, unsigned n) {} - void addParams( int m, unsigned n ) {} + void addParams(const IntVec & v) + { + if (pre) pre->addParams(params.size(), v.size()); + if (eff) eff->addParams(params.size(), v.size()); + params.insert(params.end(), v.begin(), v.end()); + } - void addParams( const IntVec & v ) { - if ( pre ) pre->addParams( params.size(), v.size() ); - if ( eff ) eff->addParams( params.size(), v.size() ); - params.insert( params.end(), v.begin(), v.end() ); - } + Condition * copy(Domain & d) { return new Action(this, d); } - Condition * copy( Domain & d ) { - return new Action( this, d ); - } + CondVec precons(); - CondVec precons(); + CondVec effects(); - CondVec effects(); + GroundVec addEffects(); - GroundVec addEffects(); - - GroundVec deleteEffects(); + GroundVec deleteEffects(); protected: + CondVec getSubconditionsFromCondition(Condition * c); - CondVec getSubconditionsFromCondition( Condition * c ); - - GroundVec getGroundsFromCondition( Condition * c, bool neg ); + GroundVec getGroundsFromCondition(Condition * c, bool neg); }; -typedef std::vector< Action * > ActionVec; +typedef std::vector ActionVec; -} } // namespaces +} // namespace pddl +} // namespace parser diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/And.h b/plansys2_pddl_parser/include/plansys2_pddl_parser/And.h index 84597c54..371b4739 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/And.h +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/And.h @@ -3,52 +3,53 @@ #include "plansys2_msgs/msg/node.hpp" #include "plansys2_msgs/msg/tree.hpp" - #include "plansys2_pddl_parser/Condition.h" -namespace parser { namespace pddl { - -class And : public Condition { - +namespace parser +{ +namespace pddl +{ +class And : public Condition +{ public: - CondVec conds; - - And() = default; + CondVec conds; - And( const And * a, Domain & d ) { - for ( unsigned i = 0; i < a->conds.size(); ++i ) - conds.push_back( a->conds[i]->copy( d ) ); - } + And() = default; - ~And() { - for ( unsigned i = 0; i < conds.size(); ++i ) - delete conds[i]; - } + And(const And * a, Domain & d) + { + for (unsigned i = 0; i < a->conds.size(); ++i) conds.push_back(a->conds[i]->copy(d)); + } - void print( std::ostream & s ) const { - for ( unsigned i = 0; i < conds.size(); ++i ) - conds[i]->print( s ); - } + ~And() + { + for (unsigned i = 0; i < conds.size(); ++i) delete conds[i]; + } - void PDDLPrint( std::ostream & s, unsigned indent, const TokenStruct< std::string > & ts, const Domain & d ) const override; + void print(std::ostream & s) const + { + for (unsigned i = 0; i < conds.size(); ++i) conds[i]->print(s); + } - plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace = {} ) const override; + void PDDLPrint( + std::ostream & s, unsigned indent, const TokenStruct & ts, + const Domain & d) const override; - void parse( Stringreader & f, TokenStruct< std::string > & ts, Domain & d ); + plansys2_msgs::msg::Node::SharedPtr getTree( + plansys2_msgs::msg::Tree & tree, const Domain & d, + const std::vector & replace = {}) const override; - void add( Condition * cond ) { - conds.push_back( cond ); - } + void parse(Stringreader & f, TokenStruct & ts, Domain & d); - void addParams( int m, unsigned n ) { - for ( unsigned i = 0; i < conds.size(); ++i ) - conds[i]->addParams( m, n ); - } + void add(Condition * cond) { conds.push_back(cond); } - Condition * copy( Domain & d ) { - return new And( this, d ); - } + void addParams(int m, unsigned n) + { + for (unsigned i = 0; i < conds.size(); ++i) conds[i]->addParams(m, n); + } + Condition * copy(Domain & d) { return new And(this, d); } }; -} } // namespaces +} // namespace pddl +} // namespace parser diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/Basic.h b/plansys2_pddl_parser/include/plansys2_pddl_parser/Basic.h index f601cd13..7d6ece0c 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/Basic.h +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/Basic.h @@ -14,80 +14,86 @@ #include #include -#define MIN( a, b ) ( ( a ) < ( b ) ? ( a ) : ( b ) ) -#define MAX( a, b ) ( ( a ) < ( b ) ? ( b ) : ( a ) ) -#define SQR( a ) ( ( a ) * ( a ) ) +#define MIN(a, b) ((a) < (b) ? (a) : (b)) +#define MAX(a, b) ((a) < (b) ? (b) : (a)) +#define SQR(a) ((a) * (a)) #define PI 3.1415926535897932384 -typedef std::set< int > IntSet; -typedef std::vector< int > IntVec; -typedef std::set< double > DoubleSet; -typedef std::vector< IntSet > SetVec; -typedef std::pair< int, int > IntPair; -typedef std::vector< IntPair > PairVec; -typedef std::map< int, IntSet > SetMap; -typedef std::vector< double > DoubleVec; -typedef std::vector< unsigned > UnsignedVec; -typedef std::vector< std::string > StringVec; -typedef std::pair< double, double > DoublePair; +typedef std::set IntSet; +typedef std::vector IntVec; +typedef std::set DoubleSet; +typedef std::vector SetVec; +typedef std::pair IntPair; +typedef std::vector PairVec; +typedef std::map SetMap; +typedef std::vector DoubleVec; +typedef std::vector UnsignedVec; +typedef std::vector StringVec; +typedef std::pair DoublePair; // create a vector of integers from lo to (hi-1) -inline IntVec incvec( unsigned lo, unsigned hi ) { - IntVec out; - for ( unsigned i = lo; i < hi; ++i ) - out.push_back( i ); - return out; +inline IntVec incvec(unsigned lo, unsigned hi) +{ + IntVec out; + for (unsigned i = lo; i < hi; ++i) out.push_back(i); + return out; } // insert all elements of a collection into a stream -template < typename T > -std::ostream & insertAll( std::ostream & stream, const T & begin, const T & end ) { - T i = begin; - stream << "["; - if ( i != end ) stream << *( i++ ); - while ( i != end ) stream << "," << *( i++ ); - return stream << "]"; +template +std::ostream & insertAll(std::ostream & stream, const T & begin, const T & end) +{ + T i = begin; + stream << "["; + if (i != end) stream << *(i++); + while (i != end) stream << "," << *(i++); + return stream << "]"; } // insert a pair into a stream -template < typename T, typename U > -std::ostream & operator<<( std::ostream & stream, const std::pair< T, U > & p ) { - return stream << "(" << p.first << "," << p.second << ")"; +template +std::ostream & operator<<(std::ostream & stream, const std::pair & p) +{ + return stream << "(" << p.first << "," << p.second << ")"; } // insert a list into a stream -template < typename T > -std::ostream & operator<<( std::ostream & stream, const std::list< T > & l ) { - return insertAll( stream, l.begin(), l.end() ); +template +std::ostream & operator<<(std::ostream & stream, const std::list & l) +{ + return insertAll(stream, l.begin(), l.end()); } // insert a map into a stream -template < typename T, typename U > -std::ostream & operator<<( std::ostream & stream, const std::map< T, U > & m ) { - return insertAll( stream, m.begin(), m.end() ); +template +std::ostream & operator<<(std::ostream & stream, const std::map & m) +{ + return insertAll(stream, m.begin(), m.end()); } // insert a multiset into a stream -template < typename T > -std::ostream & operator<<( std::ostream & stream, const std::multiset< T > & s ) { - return insertAll( stream, s.begin(), s.end() ); +template +std::ostream & operator<<(std::ostream & stream, const std::multiset & s) +{ + return insertAll(stream, s.begin(), s.end()); } // insert a set into a stream -template < typename T > -std::ostream & operator<<( std::ostream & stream, const std::set< T > & s ) { - return insertAll( stream, s.begin(), s.end() ); +template +std::ostream & operator<<(std::ostream & stream, const std::set & s) +{ + return insertAll(stream, s.begin(), s.end()); } // insert a vector into a stream -template < typename T > -std::ostream & operator<<( std::ostream & stream, const std::vector< T > &v ) { - return insertAll( stream, v.begin(), v.end() ); +template +std::ostream & operator<<(std::ostream & stream, const std::vector & v) +{ + return insertAll(stream, v.begin(), v.end()); } - -inline void tabindent( std::ostream & stream, unsigned indent ) { - for ( unsigned i = 0; i < indent; ++i ) - stream << "\t"; +inline void tabindent(std::ostream & stream, unsigned indent) +{ + for (unsigned i = 0; i < indent; ++i) stream << "\t"; } diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/CondIter.h b/plansys2_pddl_parser/include/plansys2_pddl_parser/CondIter.h index 2c211a75..7395a825 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/CondIter.h +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/CondIter.h @@ -3,42 +3,39 @@ #include "plansys2_pddl_parser/Condition.h" -namespace parser { namespace pddl { - -typedef std::list< std::pair< Condition *, unsigned > > CondList; - -class CondIter : public std::iterator< std::input_iterator_tag, Condition * > { - +namespace parser +{ +namespace pddl +{ +typedef std::list > CondList; + +class CondIter : public std::iterator +{ public: + CondList condStack; - CondList condStack; - - CondIter( Condition * c ) { - condStack.push_back( std::make_pair( c, 0 ) ); - (*this)++; - } - - CondIter( const CondIter & ci ) - : condStack( ci.condStack ) {} + CondIter(Condition * c) + { + condStack.push_back(std::make_pair(c, 0)); + (*this)++; + } - MyIterator & operator++() { - while ( condStack().size() && condStack.last().done() ) - condStack.pop_back(); + CondIter(const CondIter & ci) : condStack(ci.condStack) {} - if ( condStack().size() ) { - } + MyIterator & operator++() + { + while (condStack().size() && condStack.last().done()) condStack.pop_back(); - return *this; - } + if (condStack().size()) { + } - bool hasNext() { - return condStack.size(); - } + return *this; + } - Condition * operator*() { - return condStack.last(); - } + bool hasNext() { return condStack.size(); } + Condition * operator*() { return condStack.last(); } }; -} } // namespaces +} // namespace pddl +} // namespace parser diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/Condition.h b/plansys2_pddl_parser/include/plansys2_pddl_parser/Condition.h index 2725a4cf..fc928769 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/Condition.h +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/Condition.h @@ -5,43 +5,53 @@ #include "plansys2_msgs/msg/node.hpp" #include "plansys2_msgs/msg/tree.hpp" - #include "plansys2_pddl_parser/Basic.h" #include "plansys2_pddl_parser/Stringreader.h" -#include "plansys2_pddl_parser/Utils.h" #include "plansys2_pddl_parser/Type.h" +#include "plansys2_pddl_parser/Utils.h" -namespace parser { namespace pddl { - -class UnsupportedConstruct : public std::runtime_error { +namespace parser +{ +namespace pddl +{ +class UnsupportedConstruct : public std::runtime_error +{ public: - UnsupportedConstruct(const std::string& construct) : std::runtime_error(construct + " is not currently supported by plansys2") {} + UnsupportedConstruct(const std::string & construct) + : std::runtime_error(construct + " is not currently supported by plansys2") + { + } }; -class Condition { - +class Condition +{ public: + virtual ~Condition() {} - virtual ~Condition() {} - - virtual void print( std::ostream & stream ) const = 0; + virtual void print(std::ostream & stream) const = 0; - virtual void PDDLPrint( std::ostream & s, unsigned indent, const TokenStruct< std::string > & ts, const Domain & d ) const = 0; + virtual void PDDLPrint( + std::ostream & s, unsigned indent, const TokenStruct & ts, + const Domain & d) const = 0; - virtual plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace = {} ) const = 0; + virtual plansys2_msgs::msg::Node::SharedPtr getTree( + plansys2_msgs::msg::Tree & tree, const Domain & d, + const std::vector & replace = {}) const = 0; - virtual void parse( Stringreader & f, TokenStruct< std::string > & ts, Domain & d ) = 0; + virtual void parse(Stringreader & f, TokenStruct & ts, Domain & d) = 0; - virtual void addParams( int m, unsigned n ) = 0; + virtual void addParams(int m, unsigned n) = 0; - virtual Condition * copy( Domain & d ) = 0; + virtual Condition * copy(Domain & d) = 0; }; -inline std::ostream & operator<<( std::ostream & stream, const Condition * c ) { - c->print( stream ); - return stream; +inline std::ostream & operator<<(std::ostream & stream, const Condition * c) +{ + c->print(stream); + return stream; } -typedef std::vector< Condition * > CondVec; +typedef std::vector CondVec; -} } // namespaces +} // namespace pddl +} // namespace parser diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/Derived.h b/plansys2_pddl_parser/include/plansys2_pddl_parser/Derived.h index 49fe46c7..80e34f0b 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/Derived.h +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/Derived.h @@ -3,47 +3,49 @@ #include "plansys2_msgs/msg/node.hpp" #include "plansys2_msgs/msg/tree.hpp" - #include "plansys2_pddl_parser/Lifted.h" -namespace parser { namespace pddl { - -class Derived : public Lifted { - +namespace parser +{ +namespace pddl +{ +class Derived : public Lifted +{ public: + Condition * cond; + Lifted * lifted; - Condition * cond; - Lifted * lifted; - - Derived() - : Lifted(), cond( 0 ), lifted( 0 ) {} - - Derived( const std::string s ) - : Lifted( s ), cond( 0 ), lifted( 0 ) {} + Derived() : Lifted(), cond(0), lifted(0) {} - Derived( const Derived * z, Domain & d ); + Derived(const std::string s) : Lifted(s), cond(0), lifted(0) {} - void print( std::ostream & stream ) const { - stream << "Derived "; - ParamCond::print( stream ); - if ( cond ) cond->print( stream ); - } + Derived(const Derived * z, Domain & d); - void PDDLPrint( std::ostream & s, unsigned indent, const TokenStruct< std::string > & ts, const Domain & d ) const override; + void print(std::ostream & stream) const + { + stream << "Derived "; + ParamCond::print(stream); + if (cond) cond->print(stream); + } - plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace = {} ) const override; + void PDDLPrint( + std::ostream & s, unsigned indent, const TokenStruct & ts, + const Domain & d) const override; - void parse( Stringreader & f, TokenStruct< std::string > & ts, Domain & d ); + plansys2_msgs::msg::Node::SharedPtr getTree( + plansys2_msgs::msg::Tree & tree, const Domain & d, + const std::vector & replace = {}) const override; - void addParams( int m, unsigned n ) { - for ( unsigned i = 0; i < params.size(); ++i ) - if ( params[i] >= m ) params[i] += n; - } + void parse(Stringreader & f, TokenStruct & ts, Domain & d); - Condition * copy( Domain & d ) { - return new Derived( this, d ); - } + void addParams(int m, unsigned n) + { + for (unsigned i = 0; i < params.size(); ++i) + if (params[i] >= m) params[i] += n; + } + Condition * copy(Domain & d) { return new Derived(this, d); } }; -} } // namespaces +} // namespace pddl +} // namespace parser diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/Domain.h b/plansys2_pddl_parser/include/plansys2_pddl_parser/Domain.h index 70b3a8d8..6451425d 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/Domain.h +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/Domain.h @@ -1,624 +1,689 @@ #pragma once - #include "plansys2_pddl_parser/Task.h" - #include "plansys2_pddl_parser/TemporalAction.h" - #include "plansys2_pddl_parser/And.h" - #include "plansys2_pddl_parser/Derived.h" - #include "plansys2_pddl_parser/Exists.h" - #include "plansys2_pddl_parser/Forall.h" - #include "plansys2_pddl_parser/Function.h" - #include "plansys2_pddl_parser/GroundFunc.h" - #include "plansys2_pddl_parser/FunctionModifier.h" - - #include "plansys2_pddl_parser/Not.h" - #include "plansys2_pddl_parser/Oneof.h" - #include "plansys2_pddl_parser/Or.h" - #include "plansys2_pddl_parser/EitherType.h" - #include "plansys2_pddl_parser/When.h" +#include "plansys2_pddl_parser/And.h" +#include "plansys2_pddl_parser/Derived.h" +#include "plansys2_pddl_parser/EitherType.h" +#include "plansys2_pddl_parser/Exists.h" +#include "plansys2_pddl_parser/Forall.h" +#include "plansys2_pddl_parser/Function.h" +#include "plansys2_pddl_parser/FunctionModifier.h" +#include "plansys2_pddl_parser/GroundFunc.h" +#include "plansys2_pddl_parser/Not.h" +#include "plansys2_pddl_parser/Oneof.h" +#include "plansys2_pddl_parser/Or.h" +#include "plansys2_pddl_parser/Task.h" +#include "plansys2_pddl_parser/TemporalAction.h" +#include "plansys2_pddl_parser/When.h" #define DOMAIN_DEBUG false -namespace parser { namespace pddl { - -class Domain { +namespace parser +{ +namespace pddl +{ +class Domain +{ public: - - std::string name; // name of domain - - bool equality; // whether domain supports equality - bool strips, adl, condeffects; // whether domain is strips, adl and/or has conditional effects - bool typed, cons, costs; // whether domain is typed, has constants, has costs - bool temp, nondet, neg, disj; // whether domain is temporal, is non-deterministic, has negative precons, has disjunctive preconditions - bool universal; // whether domain has universal precons - bool fluents; // whether domains contains fluents - bool derivedpred; // whether domain contains derived predicates - - TokenStruct< Type * > types; // types - TokenStruct< Lifted * > preds; // predicates - TokenStruct< Function * > funcs; // functions - TokenStruct< Action * > actions; // actions - TokenStruct< Derived * > derived; // derived predicates - TokenStruct< Task * > tasks; // tasks - - Domain() - : equality( false ), strips( false ), adl( false ), condeffects( false ) - , typed( false ), cons( false ), costs( false ), temp( false ) - , nondet( false ), neg( false ), disj( false ), universal( false ) - , fluents( false ), derivedpred( false ) - { - types.insert( new Type( "object" ) ); // Type 0 is always "object", whether the domain is typed or not - } - - Domain( const std::string & s ) : Domain() - { - parse( s ); - } - - virtual ~Domain() { - for ( unsigned i = 0; i < types.size(); ++i ) - delete types[i]; - for ( unsigned i = 0; i < preds.size(); ++i ) - delete preds[i]; - for ( unsigned i = 0; i < funcs.size(); ++i ) - delete funcs[i]; - for ( unsigned i = 0; i < actions.size(); ++i ) - delete actions[i]; - for ( unsigned i = 0; i < derived.size(); ++i ) - delete derived[i]; - for ( unsigned i = 0; i < tasks.size(); ++i ) - delete tasks[i]; - } - - virtual void parse( const std::string & s ) { - Stringreader f( s ); - name = f.parseName( "domain" ); - - if ( DOMAIN_DEBUG ) std::cout << name << "\n"; - - for ( ; f.getChar() != ')'; f.next() ) { - f.assert_token( "(" ); - f.assert_token( ":" ); - std::string t = f.getToken(); - - if ( DOMAIN_DEBUG ) std::cout << t << "\n"; - - if (!parseBlock(t, f)) { - f.tokenExit( t ); - } - } - } - - //! Returns a boolean indicating whether the block was correctly parsed - virtual bool parseBlock(const std::string& t, Stringreader& f) { - if ( t == "requirements" ) parseRequirements( f ); - else if ( t == "types" ) parseTypes( f ); - else if ( t == "constants" ) parseConstants( f ); - else if ( t == "predicates" ) parsePredicates( f ); - else if ( t == "functions" ) parseFunctions( f ); - else if ( t == "action" ) parseAction( f ); - else if ( t == "durative-action" ) parseDurativeAction( f ); - else if ( t == "derived" ) parseDerived( f ); -// else if ( t == "axiom" ) parseAxiom( f ); - else return false; // Unknown block type - - return true; - } - - - void parseRequirements( Stringreader & f ) { - for ( f.next(); f.getChar() != ')'; f.next() ) { - f.assert_token( ":" ); - std::string s = f.getToken(); - - if ( DOMAIN_DEBUG ) std::cout << " " << s << "\n"; - - if (!parseRequirement(s)) { - f.tokenExit( s ); - } - } - - ++f.c; - } - - //! Returns a boolean indicating whether the requirement was correctly parsed - virtual bool parseRequirement( const std::string& s ) { - if ( s == "strips" ) strips = true; - else if ( s == "adl" ) adl = true; - else if ( s == "negative-preconditions" ) neg = true; - else if ( s == "conditional-effects" ) condeffects = true; - else if ( s == "typing" ) typed = true; - else if ( s == "action-cost" ) costs = true; - else if ( s == "equality" ) equality = true; - else if ( s == "durative-actions" ) temp = true; - else if ( s == "non-deterministic" ) nondet = true; - else if ( s == "universal-preconditions" ) universal = true; - else if ( s == "fluents" ) fluents = true; - else if ( s == "disjuntive-preconditions" ) disj = true; - else if ( s == "derived-predicates" ) derivedpred = true; - else return false; // Unknown requirement - - return true; - } - - // get the type corresponding to a string - Type * getType( std::string s ) { - int i = types.index( s ); - if ( i < 0 ) { - if ( s[0] == '(' ) { - i = types.insert( new EitherType( s ) ); - for ( unsigned k = 9; s[k] != ')'; ) { - unsigned e = s.find( ' ', k ); - types[i]->subtypes.push_back( getType( s.substr( k, e - k ) ) ); - k = e + 1; - } - } - else i = types.insert( new Type( s ) ); - } - return types[i]; - } - - // convert a vector of type names to integers - IntVec convertTypes( const StringVec & v ) { - IntVec out; - for ( unsigned i = 0; i < v.size(); ++i ) - out.push_back( types.index( getType( v[i] )->name ) ); - return out; - } - - void parseTypes( Stringreader & f ) { - if ( !typed ) { - std::cout << "Requirement :typing needed to define types\n"; - exit( 1 ); - } - -// if this makes it in, probably need to define new subclass of Type -// if ( costs ) insert( new Type( "NUMBER" ), tmap, types ); - - // Parse the typed list - TokenStruct< std::string > ts = f.parseTypedList( false ); - - // bit of a hack to avoid object being the supertype - if ( ts.index( "object" ) >= 0 ) { - types[0]->name = "supertype"; - types.tokenMap.clear(); - types.tokenMap["supertype"] = 0; - } - - // Relate subtypes and supertypes - for ( unsigned i = 0; i < ts.size(); ++i ) { - if (std::find(types.types.begin(), types.types.end(), ts.types[i]) == types.types.end()) { - - if ( ts.types[i].size() ) - getType( ts.types[i] )->insertSubtype( getType( ts[i] ) ); - else getType( ts[i] ); - } - } - - // By default, the supertype of a type is "object" - for ( unsigned i = 1; i < types.size(); ++i ) - if ( types[i]->supertype == 0 ) - types[0]->insertSubtype( types[i] ); - - for ( unsigned i = 0; DOMAIN_DEBUG && i < types.size(); ++i ) - std::cout << " " << types[i]; - } - - void parseConstants( Stringreader & f ) { - if ( typed && !types.size() ) { - std::cout << "Types needed before defining constants\n"; - exit( 1 ); - } - - cons = true; - - TokenStruct< std::string > ts = f.parseTypedList( true, types ); - - for ( unsigned i = 0; i < ts.size(); ++i ) - getType( ts.types[i] )->constants.insert( ts[i] ); - - for ( unsigned i = 0; DOMAIN_DEBUG && i < types.size(); ++i ) { - std::cout << " "; - if ( typed ) std::cout << " " << types[i] << ":"; - for ( unsigned j = 0; j < types[i]->constants.size(); ++j ) - std::cout << " " << types[i]->constants[j]; - std::cout << "\n"; - } - } - - void parsePredicates( Stringreader & f ) { - if ( typed && !types.size() ) { - std::cout << "Types needed before defining predicates\n"; - exit(1); - } - - for ( f.next(); f.getChar() != ')'; f.next() ) { - f.assert_token( "(" ); - if ( f.getChar() == ':' ) { - // Needed to support MA-PDDL - f.assert_token( ":private" ); - f.parseTypedList( true, types, "(" ); - - // CURRENT HACK: TOTALLY IGNORE PRIVATE !!! - --f.c; - parsePredicates( f ); - } - else { - Lifted * c = new Lifted( f.getToken() ); - c->parse( f, types[0]->constants, *this ); - - if ( DOMAIN_DEBUG ) std::cout << " " << c; - preds.insert( c ); - } - } - ++f.c; - } - - void parseFunctions( Stringreader & f ) { - if ( typed && !types.size() ) { - std::cout << "Types needed before defining functions\n"; - exit(1); - } - - for ( f.next(); f.getChar() != ')'; f.next() ) { - f.assert_token( "(" ); - Function * c = new Function( f.getToken() ); - c->parse( f, types[0]->constants, *this ); - - if ( DOMAIN_DEBUG ) std::cout << " " << c; - funcs.insert( c ); - } - ++f.c; - } - - virtual void parseAction( Stringreader & f ) { - if ( !preds.size() ) { - std::cout << "Predicates needed before defining actions\n"; - exit(1); - } - - f.next(); - Action * a = new Action( f.getToken() ); - a->parse( f, types[0]->constants, *this ); - - if ( DOMAIN_DEBUG ) std::cout << a << "\n"; - actions.insert( a ); - } - - void parseDerived( Stringreader & f ) { - if ( !preds.size() ) { - std::cout << "Predicates needed before defining derived predicates\n"; - exit(1); - } - - f.next(); - Derived * d = new Derived; - d->parse( f, types[0]->constants, *this ); - - if ( DOMAIN_DEBUG ) std::cout << d << "\n"; - derived.insert( d ); - } - - void parseDurativeAction( Stringreader & f ) { - if ( !preds.size() ) { - std::cout << "Predicates needed before defining actions\n"; - exit(1); - } - - f.next(); - Action * a = new TemporalAction( f.getToken() ); - a->parse( f, types[0]->constants, *this ); - - if ( DOMAIN_DEBUG ) std::cout << a << "\n"; - actions.insert( a ); - } - - - // Return a copy of the type structure, with newly allocated types - // This will also copy all constants and objects! - TokenStruct< Type * > copyTypes() { - TokenStruct< Type * > out; - for ( unsigned i = 0; i < types.size(); ++i ) - out.insert( types[i]->copy() ); - - for ( unsigned i = 1; i < types.size(); ++i ) { - if ( types[i]->supertype ) - out[out.index( types[i]->supertype->name )]->insertSubtype( out[i] ); - else - out[i]->copySubtypes( types[i], out ); - } - - return out; - } - - // Set the types to "otherTypes" - void setTypes( const TokenStruct< Type * > & otherTypes ) { - for ( unsigned i = 0; i < types.size(); ++i ) - delete types[i]; - types = otherTypes; - } - - // Create a type with a given supertype (default is "object") - void createType( const std::string & name, const std::string & parent = "object" ) { - Type * type = new Type( name ); - types.insert( type ); - types.get( parent )->insertSubtype( type ); - } - - // Create a constant of a given type - void createConstant( const std::string & name, const std::string & type ) { - types.get( type )->constants.insert( name ); - } - - // Create a predicate with the given name and parameter types - Lifted * createPredicate( const std::string & name, const StringVec & params = StringVec() ) { - Lifted * pred = new Lifted( name ); - for ( unsigned i = 0; i < params.size(); ++i ) - pred->params.push_back( types.index( params[i] ) ); - preds.insert( pred ); - return pred; - } - - // Create a function with the given name and parameter types - Lifted * createFunction( const std::string & name, int type, const StringVec & params = StringVec() ) { - Function * func = new Function( name, type ); - for ( unsigned i = 0; i < params.size(); ++i ) - func->params.push_back( types.index( params[i] ) ); - funcs.insert( func ); - return func; - } - - // Create an action with the given name and parameter types - Action * createAction( const std::string & name, const StringVec & params = StringVec() ) { - Action * action = new Action( name ); - for ( unsigned i = 0; i < params.size(); ++i ) - action->params.push_back( types.index( params[i] ) ); - action->pre = new And; - action->eff = new And; - actions.insert( action ); - return action; - } - - // Set the precondition of an action to "cond", converting to "And" - void setPre( const std::string & act, Condition * cond ) { - Action * action = actions.get( act ); - - And * old = dynamic_cast< And * >( cond ); - if ( old == 0 ) { - action->pre = new And; - if ( cond ) dynamic_cast< And * >( action->pre )->add( cond->copy( *this ) ); - } - else action->pre = old->copy( *this ); - } - - // Add a precondition to the action with name "act" - void addPre( bool neg, const std::string & act, const std::string & pred, const IntVec & params = IntVec() ) { - Action * action = actions.get( act ); - if ( action->pre == 0 ) action->pre = new And; - And * a = dynamic_cast< And * >( action->pre ); - if ( neg ) a->add( new Not( ground( pred, params ) ) ); - else a->add( ground( pred, params ) ); - } - - // Add an "or" precondition to the action with name "act" - void addOrPre( const std::string & act, const std::string & pred1, const std::string & pred2, - const IntVec & params1 = IntVec(), const IntVec & params2 = IntVec() ) { - Or * o = new Or; - o->first = ground( pred1, params1 ); - o->second = ground( pred2, params2 ); - Action * action = actions.get( act ); - And * a = dynamic_cast< And * >( action->pre ); - a->add( o ); - } - - // Set the precondition of an action to "cond", converting to "And" - void setEff( const std::string & act, Condition * cond ) { - Action * action = actions.get( act ); - - And * old = dynamic_cast< And * >( cond ); - if ( old == 0 ) { - action->eff = new And; - if ( cond ) dynamic_cast< And * >( action->eff )->add( cond->copy( *this ) ); - } - else action->eff = old->copy( *this ); - } - - // Add an effect to the action with name "act" - void addEff( bool neg, const std::string & act, const std::string & pred, const IntVec & params = IntVec() ) { - Action * action = actions.get( act ); - if ( action->eff == 0 ) action->eff = new And; - And * a = dynamic_cast< And * >( action->eff ); - if ( neg ) a->add( new Not( ground( pred, params ) ) ); - else a->add( ground( pred, params ) ); - } - - // Add a cost to the action with name "act", in the form of an integer - void addCost( const std::string & act, int cost ) { - Action * action = actions.get( act ); - if ( action->eff == 0 ) action->eff = new And; - And * a = dynamic_cast< And * >( action->eff ); - a->add( new Increase( cost ) ); - } - - // Add a cost to the action with name "act", in the form of a function - void addCost( const std::string & act, const std::string & func, const IntVec & params = IntVec() ) { - Action * action = actions.get( act ); - if ( action->eff == 0 ) action->eff = new And; - And * a = dynamic_cast< And * >( action->eff ); - a->add( new Increase( funcs.get( func ), params ) ); - } - - void addFunctionModifier( const std::string & act, FunctionModifier * fm ) { - Action * action = actions.get( act ); - if ( action->eff == 0 ) action->eff = new And; - And * a = dynamic_cast< And * >( action->eff ); - a->add( fm ); - } - - // Create a ground condition with the given name - Ground * ground( const std::string & name, const IntVec & params = IntVec() ) { - if ( preds.index( name ) < 0 ) { - std::cout << "Creating a ground condition " << name << params; - std::cout << " failed since the predicate " << name << " does not exist!\n"; - std::exit( 1 ); - } - return new Ground( preds[preds.index( name )], params ); - } - - // Return the list of type names corresponding to a parameter list - StringVec typeList( ParamCond * c ) { - StringVec out; - for ( unsigned i = 0; i < c->params.size(); ++i ) - out.push_back( types[c->params[i]]->name ); - return out; - } - - // Return the list of object names corresponding to a ground fluent - StringVec objectList( Ground * g ) { - StringVec out; - for ( unsigned i = 0; i < g->params.size(); ++i ) - out.push_back( types[g->lifted->params[i]]->object( g->params[i] ).first ); - return out; - } - - // Add parameters to an action - void addParams( const std::string & name, const StringVec & v ) { - actions.get( name )->addParams( convertTypes( v ) ); - } - - // Assert that one type is a subtype of another - bool assertSubtype( int t1, int t2 ) { - for ( Type * type = types[t1]; type != 0; type = type->supertype ) - if ( type->name == types[t2]->name ) return 1; - return 0; - } - - // Check whether the given string represents a valid constant in the model - bool isConstant( const std::string & name) { - bool res = false; - for (int t = 0; t < types.size() && !res; t++) { - for (int c = 0; c < types[t]->constants.size() && !res; c++) { - if (types[t]->constants[c] == name) res = true; - } - } - return res; - } - - IntPair constantTypeIdConstId(const std::string & name) { - int t, c; - bool found = false; - for (t = 0; t < types.size() && !found; t++) { - for (c = 0; c < types[t]->constants.size() && !found; c++) { - if (name == types[t]->constants[c]) found = true; - } - } - if (!found) return IntPair(-1,-1); - return IntPair(c-1,t-1); - } - - // return the index of a constant for a given type - int constantIndex( const std::string & name, const std::string & type ) { - return types.get( type )->parseConstant( name ).second; - } - - //! Prints a PDDL representation of the object to the given stream. - friend std::ostream& operator<<(std::ostream &os, const Domain& o) { return o.print(os); } - virtual std::ostream& print(std::ostream& os) const { - os << "( define ( domain " << name << " )\n"; - print_requirements(os); - - if ( typed ) { - os << "( :types\n"; - for ( unsigned i = 1; i < types.size(); ++i ) - types[i]->PDDLPrint( os ); - os << ")\n"; - } - - if ( cons ) { - os << "( :constants\n"; - for ( unsigned i = 0; i < types.size(); ++i ) - if ( types[i]->constants.size() ) { - os << "\t"; - for ( unsigned j = 0; j < types[i]->constants.size(); ++j ) - os << types[i]->constants[j] << " "; - if ( typed ) - os << "- " << types[i]->name; - os << "\n"; - } - os << ")\n"; - } - - os << "( :predicates\n"; - for ( unsigned i = 0; i < preds.size(); ++i ) { - preds[i]->PDDLPrint( os, 1, TokenStruct< std::string >(), *this ); - os << "\n"; - } - os << ")\n"; - - if ( funcs.size() ) { - os << "( :functions\n"; - for ( unsigned i = 0; i < funcs.size(); ++i ) { - funcs[i]->PDDLPrint( os, 1, TokenStruct< std::string >(), *this ); - os << "\n"; - } - os << ")\n"; - } - - for ( unsigned i = 0; i < actions.size(); ++i ) - actions[i]->PDDLPrint( os, 0, TokenStruct< std::string >(), *this ); - - for ( unsigned i = 0; i < derived.size(); ++i ) - derived[i]->PDDLPrint( os, 0, TokenStruct< std::string >(), *this ); - - print_addtional_blocks(os); - - os << ")\n"; - return os; - } - - virtual std::ostream& print_requirements(std::ostream& os) const { - os << "( :requirements"; - if ( equality ) os << " :equality"; - if ( strips ) os << " :strips"; - if ( costs ) os << " :action-cost"; - if ( adl ) os << " :adl"; - if ( neg ) os << " :negative-preconditions"; - if ( condeffects ) os << " :conditional-effects"; - if ( typed ) os << " :typing"; - if ( temp ) os << " :durative-actions"; - if ( nondet ) os << " :non-deterministic"; - if ( universal ) os << " :universal-preconditions"; - if ( fluents ) os << " :fluents"; - if ( disj ) os << " :disjuntive-preconditions"; - if ( derivedpred ) os << " :derived-predicates"; - os << " )\n"; - return os; - } - - virtual std::ostream& print_addtional_blocks(std::ostream& os) const { return os; } - - virtual Condition * createCondition( Stringreader & f ) { - std::string s = f.getToken(); - - if ( s == "and" ) return new And; - if ( s == "exists" ) return new Exists; - if ( s == "forall" ) return new Forall; - if ( s == "assign" ) return new Assign; - if ( s == "increase" ) return new Increase; - if ( s == "decrease" ) return new Decrease; - if ( s == "not" ) return new Not; - if ( s == "oneof" ) return new Oneof; - if ( s == "or" ) return new Or; - if ( s == "when" ) return new When; - if ( s == "=" || s == ">=" || s == ">" || s == "<=" || s == "<" ) return new CompositeExpression( s ); - - int i = preds.index( s ); - if ( i >= 0 ) return new Ground( preds[i] ); - - f.tokenExit( s ); - - return 0; - } + std::string name; // name of domain + + bool equality; // whether domain supports equality + bool strips, adl, condeffects; // whether domain is strips, adl and/or has + // conditional effects + bool typed, cons, costs; // whether domain is typed, has constants, has costs + bool temp, nondet, neg, + disj; // whether domain is temporal, is non-deterministic, has negative + // precons, has disjunctive preconditions + bool universal; // whether domain has universal precons + bool fluents; // whether domains contains fluents + bool derivedpred; // whether domain contains derived predicates + + TokenStruct types; // types + TokenStruct preds; // predicates + TokenStruct funcs; // functions + TokenStruct actions; // actions + TokenStruct derived; // derived predicates + TokenStruct tasks; // tasks + + Domain() + : equality(false), + strips(false), + adl(false), + condeffects(false), + typed(false), + cons(false), + costs(false), + temp(false), + nondet(false), + neg(false), + disj(false), + universal(false), + fluents(false), + derivedpred(false) + { + types.insert(new Type("object")); // Type 0 is always "object", whether the + // domain is typed or not + } + + Domain(const std::string & s) : Domain() { parse(s); } + + virtual ~Domain() + { + for (unsigned i = 0; i < types.size(); ++i) delete types[i]; + for (unsigned i = 0; i < preds.size(); ++i) delete preds[i]; + for (unsigned i = 0; i < funcs.size(); ++i) delete funcs[i]; + for (unsigned i = 0; i < actions.size(); ++i) delete actions[i]; + for (unsigned i = 0; i < derived.size(); ++i) delete derived[i]; + for (unsigned i = 0; i < tasks.size(); ++i) delete tasks[i]; + } + + virtual void parse(const std::string & s) + { + Stringreader f(s); + name = f.parseName("domain"); + + if (DOMAIN_DEBUG) std::cout << name << "\n"; + + for (; f.getChar() != ')'; f.next()) { + f.assert_token("("); + f.assert_token(":"); + std::string t = f.getToken(); + + if (DOMAIN_DEBUG) std::cout << t << "\n"; + + if (!parseBlock(t, f)) { + f.tokenExit(t); + } + } + } + + //! Returns a boolean indicating whether the block was correctly parsed + virtual bool parseBlock(const std::string & t, Stringreader & f) + { + if (t == "requirements") + parseRequirements(f); + else if (t == "types") + parseTypes(f); + else if (t == "constants") + parseConstants(f); + else if (t == "predicates") + parsePredicates(f); + else if (t == "functions") + parseFunctions(f); + else if (t == "action") + parseAction(f); + else if (t == "durative-action") + parseDurativeAction(f); + else if (t == "derived") + parseDerived(f); + // else if ( t == "axiom" ) parseAxiom( f ); + else + return false; // Unknown block type + + return true; + } + + void parseRequirements(Stringreader & f) + { + for (f.next(); f.getChar() != ')'; f.next()) { + f.assert_token(":"); + std::string s = f.getToken(); + + if (DOMAIN_DEBUG) std::cout << " " << s << "\n"; + + if (!parseRequirement(s)) { + f.tokenExit(s); + } + } + + ++f.c; + } + + //! Returns a boolean indicating whether the requirement was correctly parsed + virtual bool parseRequirement(const std::string & s) + { + if (s == "strips") + strips = true; + else if (s == "adl") + adl = true; + else if (s == "negative-preconditions") + neg = true; + else if (s == "conditional-effects") + condeffects = true; + else if (s == "typing") + typed = true; + else if (s == "action-cost") + costs = true; + else if (s == "equality") + equality = true; + else if (s == "durative-actions") + temp = true; + else if (s == "non-deterministic") + nondet = true; + else if (s == "universal-preconditions") + universal = true; + else if (s == "fluents") + fluents = true; + else if (s == "disjuntive-preconditions") + disj = true; + else if (s == "derived-predicates") + derivedpred = true; + else + return false; // Unknown requirement + + return true; + } + + // get the type corresponding to a string + Type * getType(std::string s) + { + int i = types.index(s); + if (i < 0) { + if (s[0] == '(') { + i = types.insert(new EitherType(s)); + for (unsigned k = 9; s[k] != ')';) { + unsigned e = s.find(' ', k); + types[i]->subtypes.push_back(getType(s.substr(k, e - k))); + k = e + 1; + } + } else + i = types.insert(new Type(s)); + } + return types[i]; + } + + // convert a vector of type names to integers + IntVec convertTypes(const StringVec & v) + { + IntVec out; + for (unsigned i = 0; i < v.size(); ++i) out.push_back(types.index(getType(v[i])->name)); + return out; + } + + void parseTypes(Stringreader & f) + { + if (!typed) { + std::cout << "Requirement :typing needed to define types\n"; + exit(1); + } + + // if this makes it in, probably need to define new subclass of + // Type if ( costs ) insert( new Type( "NUMBER" ), tmap, types + // ); + + // Parse the typed list + TokenStruct ts = f.parseTypedList(false); + + // bit of a hack to avoid object being the supertype + if (ts.index("object") >= 0) { + types[0]->name = "supertype"; + types.tokenMap.clear(); + types.tokenMap["supertype"] = 0; + } + + // Relate subtypes and supertypes + for (unsigned i = 0; i < ts.size(); ++i) { + if (std::find(types.types.begin(), types.types.end(), ts.types[i]) == types.types.end()) { + if (ts.types[i].size()) + getType(ts.types[i])->insertSubtype(getType(ts[i])); + else + getType(ts[i]); + } + } + + // By default, the supertype of a type is "object" + for (unsigned i = 1; i < types.size(); ++i) + if (types[i]->supertype == 0) types[0]->insertSubtype(types[i]); + + for (unsigned i = 0; DOMAIN_DEBUG && i < types.size(); ++i) std::cout << " " << types[i]; + } + + void parseConstants(Stringreader & f) + { + if (typed && !types.size()) { + std::cout << "Types needed before defining constants\n"; + exit(1); + } + + cons = true; + + TokenStruct ts = f.parseTypedList(true, types); + + for (unsigned i = 0; i < ts.size(); ++i) getType(ts.types[i])->constants.insert(ts[i]); + + for (unsigned i = 0; DOMAIN_DEBUG && i < types.size(); ++i) { + std::cout << " "; + if (typed) std::cout << " " << types[i] << ":"; + for (unsigned j = 0; j < types[i]->constants.size(); ++j) + std::cout << " " << types[i]->constants[j]; + std::cout << "\n"; + } + } + + void parsePredicates(Stringreader & f) + { + if (typed && !types.size()) { + std::cout << "Types needed before defining predicates\n"; + exit(1); + } + + for (f.next(); f.getChar() != ')'; f.next()) { + f.assert_token("("); + if (f.getChar() == ':') { + // Needed to support MA-PDDL + f.assert_token(":private"); + f.parseTypedList(true, types, "("); + + // CURRENT HACK: TOTALLY IGNORE PRIVATE !!! + --f.c; + parsePredicates(f); + } else { + Lifted * c = new Lifted(f.getToken()); + c->parse(f, types[0]->constants, *this); + + if (DOMAIN_DEBUG) std::cout << " " << c; + preds.insert(c); + } + } + ++f.c; + } + + void parseFunctions(Stringreader & f) + { + if (typed && !types.size()) { + std::cout << "Types needed before defining functions\n"; + exit(1); + } + + for (f.next(); f.getChar() != ')'; f.next()) { + f.assert_token("("); + Function * c = new Function(f.getToken()); + c->parse(f, types[0]->constants, *this); + + if (DOMAIN_DEBUG) std::cout << " " << c; + funcs.insert(c); + } + ++f.c; + } + + virtual void parseAction(Stringreader & f) + { + if (!preds.size()) { + std::cout << "Predicates needed before defining actions\n"; + exit(1); + } + + f.next(); + Action * a = new Action(f.getToken()); + a->parse(f, types[0]->constants, *this); + + if (DOMAIN_DEBUG) std::cout << a << "\n"; + actions.insert(a); + } + + void parseDerived(Stringreader & f) + { + if (!preds.size()) { + std::cout << "Predicates needed before defining derived predicates\n"; + exit(1); + } + + f.next(); + Derived * d = new Derived; + d->parse(f, types[0]->constants, *this); + + if (DOMAIN_DEBUG) std::cout << d << "\n"; + derived.insert(d); + } + + void parseDurativeAction(Stringreader & f) + { + if (!preds.size()) { + std::cout << "Predicates needed before defining actions\n"; + exit(1); + } + + f.next(); + Action * a = new TemporalAction(f.getToken()); + a->parse(f, types[0]->constants, *this); + + if (DOMAIN_DEBUG) std::cout << a << "\n"; + actions.insert(a); + } + + // Return a copy of the type structure, with newly allocated types + // This will also copy all constants and objects! + TokenStruct copyTypes() + { + TokenStruct out; + for (unsigned i = 0; i < types.size(); ++i) out.insert(types[i]->copy()); + + for (unsigned i = 1; i < types.size(); ++i) { + if (types[i]->supertype) + out[out.index(types[i]->supertype->name)]->insertSubtype(out[i]); + else + out[i]->copySubtypes(types[i], out); + } + + return out; + } + + // Set the types to "otherTypes" + void setTypes(const TokenStruct & otherTypes) + { + for (unsigned i = 0; i < types.size(); ++i) delete types[i]; + types = otherTypes; + } + + // Create a type with a given supertype (default is "object") + void createType(const std::string & name, const std::string & parent = "object") + { + Type * type = new Type(name); + types.insert(type); + types.get(parent)->insertSubtype(type); + } + + // Create a constant of a given type + void createConstant(const std::string & name, const std::string & type) + { + types.get(type)->constants.insert(name); + } + + // Create a predicate with the given name and parameter types + Lifted * createPredicate(const std::string & name, const StringVec & params = StringVec()) + { + Lifted * pred = new Lifted(name); + for (unsigned i = 0; i < params.size(); ++i) pred->params.push_back(types.index(params[i])); + preds.insert(pred); + return pred; + } + + // Create a function with the given name and parameter types + Lifted * createFunction( + const std::string & name, int type, const StringVec & params = StringVec()) + { + Function * func = new Function(name, type); + for (unsigned i = 0; i < params.size(); ++i) func->params.push_back(types.index(params[i])); + funcs.insert(func); + return func; + } + + // Create an action with the given name and parameter types + Action * createAction(const std::string & name, const StringVec & params = StringVec()) + { + Action * action = new Action(name); + for (unsigned i = 0; i < params.size(); ++i) action->params.push_back(types.index(params[i])); + action->pre = new And; + action->eff = new And; + actions.insert(action); + return action; + } + + // Set the precondition of an action to "cond", converting to "And" + void setPre(const std::string & act, Condition * cond) + { + Action * action = actions.get(act); + + And * old = dynamic_cast(cond); + if (old == 0) { + action->pre = new And; + if (cond) dynamic_cast(action->pre)->add(cond->copy(*this)); + } else + action->pre = old->copy(*this); + } + + // Add a precondition to the action with name "act" + void addPre( + bool neg, const std::string & act, const std::string & pred, const IntVec & params = IntVec()) + { + Action * action = actions.get(act); + if (action->pre == 0) action->pre = new And; + And * a = dynamic_cast(action->pre); + if (neg) + a->add(new Not(ground(pred, params))); + else + a->add(ground(pred, params)); + } + + // Add an "or" precondition to the action with name "act" + void addOrPre( + const std::string & act, const std::string & pred1, const std::string & pred2, + const IntVec & params1 = IntVec(), const IntVec & params2 = IntVec()) + { + Or * o = new Or; + o->first = ground(pred1, params1); + o->second = ground(pred2, params2); + Action * action = actions.get(act); + And * a = dynamic_cast(action->pre); + a->add(o); + } + + // Set the precondition of an action to "cond", converting to "And" + void setEff(const std::string & act, Condition * cond) + { + Action * action = actions.get(act); + + And * old = dynamic_cast(cond); + if (old == 0) { + action->eff = new And; + if (cond) dynamic_cast(action->eff)->add(cond->copy(*this)); + } else + action->eff = old->copy(*this); + } + + // Add an effect to the action with name "act" + void addEff( + bool neg, const std::string & act, const std::string & pred, const IntVec & params = IntVec()) + { + Action * action = actions.get(act); + if (action->eff == 0) action->eff = new And; + And * a = dynamic_cast(action->eff); + if (neg) + a->add(new Not(ground(pred, params))); + else + a->add(ground(pred, params)); + } + + // Add a cost to the action with name "act", in the form of an integer + void addCost(const std::string & act, int cost) + { + Action * action = actions.get(act); + if (action->eff == 0) action->eff = new And; + And * a = dynamic_cast(action->eff); + a->add(new Increase(cost)); + } + + // Add a cost to the action with name "act", in the form of a function + void addCost(const std::string & act, const std::string & func, const IntVec & params = IntVec()) + { + Action * action = actions.get(act); + if (action->eff == 0) action->eff = new And; + And * a = dynamic_cast(action->eff); + a->add(new Increase(funcs.get(func), params)); + } + + void addFunctionModifier(const std::string & act, FunctionModifier * fm) + { + Action * action = actions.get(act); + if (action->eff == 0) action->eff = new And; + And * a = dynamic_cast(action->eff); + a->add(fm); + } + + // Create a ground condition with the given name + Ground * ground(const std::string & name, const IntVec & params = IntVec()) + { + if (preds.index(name) < 0) { + std::cout << "Creating a ground condition " << name << params; + std::cout << " failed since the predicate " << name << " does not exist!\n"; + std::exit(1); + } + return new Ground(preds[preds.index(name)], params); + } + + // Return the list of type names corresponding to a parameter list + StringVec typeList(ParamCond * c) + { + StringVec out; + for (unsigned i = 0; i < c->params.size(); ++i) out.push_back(types[c->params[i]]->name); + return out; + } + + // Return the list of object names corresponding to a ground fluent + StringVec objectList(Ground * g) + { + StringVec out; + for (unsigned i = 0; i < g->params.size(); ++i) + out.push_back(types[g->lifted->params[i]]->object(g->params[i]).first); + return out; + } + + // Add parameters to an action + void addParams(const std::string & name, const StringVec & v) + { + actions.get(name)->addParams(convertTypes(v)); + } + + // Assert that one type is a subtype of another + bool assertSubtype(int t1, int t2) + { + for (Type * type = types[t1]; type != 0; type = type->supertype) + if (type->name == types[t2]->name) return 1; + return 0; + } + + // Check whether the given string represents a valid constant in the model + bool isConstant(const std::string & name) + { + bool res = false; + for (int t = 0; t < types.size() && !res; t++) { + for (int c = 0; c < types[t]->constants.size() && !res; c++) { + if (types[t]->constants[c] == name) res = true; + } + } + return res; + } + + IntPair constantTypeIdConstId(const std::string & name) + { + int t, c; + bool found = false; + for (t = 0; t < types.size() && !found; t++) { + for (c = 0; c < types[t]->constants.size() && !found; c++) { + if (name == types[t]->constants[c]) found = true; + } + } + if (!found) return IntPair(-1, -1); + return IntPair(c - 1, t - 1); + } + + // return the index of a constant for a given type + int constantIndex(const std::string & name, const std::string & type) + { + return types.get(type)->parseConstant(name).second; + } + + //! Prints a PDDL representation of the object to the given stream. + friend std::ostream & operator<<(std::ostream & os, const Domain & o) { return o.print(os); } + virtual std::ostream & print(std::ostream & os) const + { + os << "( define ( domain " << name << " )\n"; + print_requirements(os); + + if (typed) { + os << "( :types\n"; + for (unsigned i = 1; i < types.size(); ++i) types[i]->PDDLPrint(os); + os << ")\n"; + } + + if (cons) { + os << "( :constants\n"; + for (unsigned i = 0; i < types.size(); ++i) + if (types[i]->constants.size()) { + os << "\t"; + for (unsigned j = 0; j < types[i]->constants.size(); ++j) + os << types[i]->constants[j] << " "; + if (typed) os << "- " << types[i]->name; + os << "\n"; + } + os << ")\n"; + } + + os << "( :predicates\n"; + for (unsigned i = 0; i < preds.size(); ++i) { + preds[i]->PDDLPrint(os, 1, TokenStruct(), *this); + os << "\n"; + } + os << ")\n"; + + if (funcs.size()) { + os << "( :functions\n"; + for (unsigned i = 0; i < funcs.size(); ++i) { + funcs[i]->PDDLPrint(os, 1, TokenStruct(), *this); + os << "\n"; + } + os << ")\n"; + } + + for (unsigned i = 0; i < actions.size(); ++i) + actions[i]->PDDLPrint(os, 0, TokenStruct(), *this); + + for (unsigned i = 0; i < derived.size(); ++i) + derived[i]->PDDLPrint(os, 0, TokenStruct(), *this); + + print_addtional_blocks(os); + + os << ")\n"; + return os; + } + + virtual std::ostream & print_requirements(std::ostream & os) const + { + os << "( :requirements"; + if (equality) os << " :equality"; + if (strips) os << " :strips"; + if (costs) os << " :action-cost"; + if (adl) os << " :adl"; + if (neg) os << " :negative-preconditions"; + if (condeffects) os << " :conditional-effects"; + if (typed) os << " :typing"; + if (temp) os << " :durative-actions"; + if (nondet) os << " :non-deterministic"; + if (universal) os << " :universal-preconditions"; + if (fluents) os << " :fluents"; + if (disj) os << " :disjuntive-preconditions"; + if (derivedpred) os << " :derived-predicates"; + os << " )\n"; + return os; + } + + virtual std::ostream & print_addtional_blocks(std::ostream & os) const { return os; } + + virtual Condition * createCondition(Stringreader & f) + { + std::string s = f.getToken(); + + if (s == "and") return new And; + if (s == "exists") return new Exists; + if (s == "forall") return new Forall; + if (s == "assign") return new Assign; + if (s == "increase") return new Increase; + if (s == "decrease") return new Decrease; + if (s == "not") return new Not; + if (s == "oneof") return new Oneof; + if (s == "or") return new Or; + if (s == "when") return new When; + if (s == "=" || s == ">=" || s == ">" || s == "<=" || s == "<") + return new CompositeExpression(s); + + int i = preds.index(s); + if (i >= 0) return new Ground(preds[i]); + + f.tokenExit(s); + + return 0; + } }; -} } // namespaces +} // namespace pddl +} // namespace parser diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/EitherType.h b/plansys2_pddl_parser/include/plansys2_pddl_parser/EitherType.h index 52727c13..7dc3509a 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/EitherType.h +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/EitherType.h @@ -3,31 +3,28 @@ #include "plansys2_pddl_parser/Type.h" -namespace parser { namespace pddl { - -class EitherType : public Type { - +namespace parser +{ +namespace pddl +{ +class EitherType : public Type +{ public: + EitherType(const std::string & s) : Type(s) {} - EitherType( const std::string & s ) - : Type( s ) {} - - EitherType( const EitherType * t ) - : Type( t ) {} - - std::string getName() const { - std::string out = "either"; - for ( unsigned i = 0; i < subtypes.size(); ++i ) - out += "_" + subtypes[i]->getName(); - return out; - } + EitherType(const EitherType * t) : Type(t) {} - void PDDLPrint( std::ostream & s ) const override {} + std::string getName() const + { + std::string out = "either"; + for (unsigned i = 0; i < subtypes.size(); ++i) out += "_" + subtypes[i]->getName(); + return out; + } - Type * copy() { - return new EitherType( this ); - } + void PDDLPrint(std::ostream & s) const override {} + Type * copy() { return new EitherType(this); } }; -} } // namespaces +} // namespace pddl +} // namespace parser diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/Exists.h b/plansys2_pddl_parser/include/plansys2_pddl_parser/Exists.h index 810bd4d0..6727dbdb 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/Exists.h +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/Exists.h @@ -3,48 +3,49 @@ #include "plansys2_msgs/msg/node.hpp" #include "plansys2_msgs/msg/tree.hpp" - #include "plansys2_pddl_parser/ParamCond.h" -namespace parser { namespace pddl { - -class Exists : public ParamCond { - +namespace parser +{ +namespace pddl +{ +class Exists : public ParamCond +{ public: + Condition * cond; - Condition * cond; - - Exists() - : cond( 0 ) {} - - Exists( const Exists * e, Domain & d ) - : ParamCond( e ), cond( 0 ) { - if ( e->cond ) cond = e->cond->copy( d ); - } + Exists() : cond(0) {} - ~Exists() { - if ( cond ) delete cond; - } + Exists(const Exists * e, Domain & d) : ParamCond(e), cond(0) + { + if (e->cond) cond = e->cond->copy(d); + } - void print( std::ostream & s ) const { - s << "Exists" << params << ":\n"; - cond->print( s ); - } + ~Exists() + { + if (cond) delete cond; + } - void PDDLPrint( std::ostream & s, unsigned indent, const TokenStruct< std::string > & ts, const Domain & d ) const override; + void print(std::ostream & s) const + { + s << "Exists" << params << ":\n"; + cond->print(s); + } - plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace = {} ) const override; + void PDDLPrint( + std::ostream & s, unsigned indent, const TokenStruct & ts, + const Domain & d) const override; - void parse( Stringreader & f, TokenStruct< std::string > & ts, Domain & d ); + plansys2_msgs::msg::Node::SharedPtr getTree( + plansys2_msgs::msg::Tree & tree, const Domain & d, + const std::vector & replace = {}) const override; - void addParams( int m, unsigned n ) { - cond->addParams( m, n ); - } + void parse(Stringreader & f, TokenStruct & ts, Domain & d); - Condition * copy( Domain & d ) { - return new Exists( this, d ); - } + void addParams(int m, unsigned n) { cond->addParams(m, n); } + Condition * copy(Domain & d) { return new Exists(this, d); } }; -} } // namespaces +} // namespace pddl +} // namespace parser diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/Expression.h b/plansys2_pddl_parser/include/plansys2_pddl_parser/Expression.h index 10d2593c..ae4e8653 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/Expression.h +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/Expression.h @@ -3,308 +3,320 @@ #include "plansys2_msgs/msg/node.hpp" #include "plansys2_msgs/msg/tree.hpp" - #include "plansys2_pddl_parser/Condition.h" -namespace parser { namespace pddl { - +namespace parser +{ +namespace pddl +{ class Instance; -class Expression : public Condition { - +class Expression : public Condition +{ public: + virtual ~Expression() {} + virtual std::string info() const = 0; + virtual double evaluate() = 0; + virtual double evaluate(Instance & ins, const StringVec & par) = 0; + virtual IntSet params() = 0; - virtual ~Expression() {} - virtual std::string info() const = 0; - virtual double evaluate() = 0; - virtual double evaluate( Instance & ins, const StringVec & par ) = 0; - virtual IntSet params() = 0; - - // inherit - virtual void print( std::ostream & stream ) const { - stream << info(); - } + // inherit + virtual void print(std::ostream & stream) const { stream << info(); } - virtual void parse( Stringreader & f, TokenStruct< std::string > & ts, Domain & d ) {} - virtual void addParams( int m, unsigned n ) {} + virtual void parse(Stringreader & f, TokenStruct & ts, Domain & d) {} + virtual void addParams(int m, unsigned n) {} }; -Expression * createExpression( Stringreader & f, TokenStruct< std::string > & ts, Domain & d ); - -class CompositeExpression : public Expression { +Expression * createExpression(Stringreader & f, TokenStruct & ts, Domain & d); +class CompositeExpression : public Expression +{ public: + std::string op; + Expression * left; + Expression * right; - std::string op; - Expression * left; - Expression * right; - - CompositeExpression( const std::string& c ) : op( c ) {} - - CompositeExpression( const std::string& c, Expression * l, Expression * r ) : op( c ), left( l ), right( r ) {} - - ~CompositeExpression() { - delete left; - delete right; - } - - void parse( Stringreader & f, TokenStruct< std::string > & ts, Domain & d ) { - f.next(); - left = createExpression( f, ts, d ); - right = createExpression( f, ts, d ); - f.next(); - f.assert_token( ")" ); - } - - std::string info() const { - std::ostringstream os; - os << "(" << op << " " << left->info() << " " << right->info() << ")"; - return os.str(); - } - - void PDDLPrint( std::ostream & s, unsigned indent, const TokenStruct< std::string > & ts, const Domain & d ) const override { - tabindent(s, indent); - s << "( " << op << " "; - left->PDDLPrint( s, indent, ts, d ); - s << " "; - right->PDDLPrint( s, indent, ts, d ); - s << " )"; - } - - plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace = {} ) const override { - plansys2_msgs::msg::Node::SharedPtr node = std::make_shared(); - node->node_type = plansys2_msgs::msg::Node::EXPRESSION; - node->expression_type = getExprType(op); - node->node_id = tree.nodes.size(); - tree.nodes.push_back(*node); - - plansys2_msgs::msg::Node::SharedPtr left_child = left->getTree(tree, d, replace); - tree.nodes[node->node_id].children.push_back(left_child->node_id); - - plansys2_msgs::msg::Node::SharedPtr right_child = right->getTree(tree, d, replace); - tree.nodes[node->node_id].children.push_back(right_child->node_id); - - return node; - } - - double compute( double x, double y ) { - double res = 0; - - if ( op == "+" ) res = x + y; - else if ( op == "-" ) res = x - y; - else if ( op == "*" ) res = x * y; - else if ( op == "/" ) res = ( y == 0 ? 0 : x / y ); - - return res; - } - - double evaluate() { - return compute( left->evaluate(), right->evaluate() ); - } - - double evaluate( Instance & ins, const StringVec & par ) { - return compute( left->evaluate( ins, par ), right->evaluate( ins, par ) ); - } - - IntSet params() { - IntSet lpars = left->params(); - IntSet rpars = right->params(); - lpars.insert( rpars.begin(), rpars.end() ); - return lpars; - } - - Condition * copy( Domain & d ) { - Expression * cleft = dynamic_cast< Expression * >( left->copy( d ) ); - Expression * cright = dynamic_cast< Expression * >( right->copy( d ) ); - return new CompositeExpression( op, cleft, cright ); - } -}; + CompositeExpression(const std::string & c) : op(c) {} -class FunctionExpression : public Expression { + CompositeExpression(const std::string & c, Expression * l, Expression * r) + : op(c), left(l), right(r) + { + } -public: + ~CompositeExpression() + { + delete left; + delete right; + } - ParamCond * fun; - std::vector constants; + void parse(Stringreader & f, TokenStruct & ts, Domain & d) + { + f.next(); + left = createExpression(f, ts, d); + right = createExpression(f, ts, d); + f.next(); + f.assert_token(")"); + } - FunctionExpression( ParamCond * c ) : fun( c ) { - constants.clear(); - constants.resize(c->params.size()); - std::fill(constants.begin(), constants.end(), nullptr); + std::string info() const + { + std::ostringstream os; + os << "(" << op << " " << left->info() << " " << right->info() << ")"; + return os.str(); } - FunctionExpression( ParamCond * c, std::vector e) : fun( c ), constants(e) {} - ~FunctionExpression() { - delete fun; - } + void PDDLPrint( + std::ostream & s, unsigned indent, const TokenStruct & ts, + const Domain & d) const override + { + tabindent(s, indent); + s << "( " << op << " "; + left->PDDLPrint(s, indent, ts, d); + s << " "; + right->PDDLPrint(s, indent, ts, d); + s << " )"; + } - std::string info() const { - std::ostringstream os; - os << "(" << fun->name << fun->params << ")"; - return os.str(); - } + plansys2_msgs::msg::Node::SharedPtr getTree( + plansys2_msgs::msg::Tree & tree, const Domain & d, + const std::vector & replace = {}) const override + { + plansys2_msgs::msg::Node::SharedPtr node = std::make_shared(); + node->node_type = plansys2_msgs::msg::Node::EXPRESSION; + node->expression_type = getExprType(op); + node->node_id = tree.nodes.size(); + tree.nodes.push_back(*node); - void PDDLPrint( std::ostream & s, unsigned indent, const TokenStruct< std::string > & ts, const Domain & d ) const override; + plansys2_msgs::msg::Node::SharedPtr left_child = left->getTree(tree, d, replace); + tree.nodes[node->node_id].children.push_back(left_child->node_id); - plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace = {} ) const override; + plansys2_msgs::msg::Node::SharedPtr right_child = right->getTree(tree, d, replace); + tree.nodes[node->node_id].children.push_back(right_child->node_id); - double evaluate() { return 1; } + return node; + } - double evaluate( Instance & ins, const StringVec & par ); + double compute(double x, double y) + { + double res = 0; - IntSet params() { - return IntSet( fun->params.begin(), fun->params.end() ); - } + if (op == "+") + res = x + y; + else if (op == "-") + res = x - y; + else if (op == "*") + res = x * y; + else if (op == "/") + res = (y == 0 ? 0 : x / y); - Condition * copy( Domain & d ) { - return new FunctionExpression( dynamic_cast< ParamCond * >( fun->copy( d ) ) ); - } -}; + return res; + } + + double evaluate() { return compute(left->evaluate(), right->evaluate()); } + + double evaluate(Instance & ins, const StringVec & par) + { + return compute(left->evaluate(ins, par), right->evaluate(ins, par)); + } + + IntSet params() + { + IntSet lpars = left->params(); + IntSet rpars = right->params(); + lpars.insert(rpars.begin(), rpars.end()); + return lpars; + } -class ValueExpression : public Expression { + Condition * copy(Domain & d) + { + Expression * cleft = dynamic_cast(left->copy(d)); + Expression * cright = dynamic_cast(right->copy(d)); + return new CompositeExpression(op, cleft, cright); + } +}; +class FunctionExpression : public Expression +{ public: + ParamCond * fun; + std::vector constants; - double value; + FunctionExpression(ParamCond * c) : fun(c) + { + constants.clear(); + constants.resize(c->params.size()); + std::fill(constants.begin(), constants.end(), nullptr); + } + FunctionExpression(ParamCond * c, std::vector e) : fun(c), constants(e) {} - ValueExpression( double v ) : value( v ) {} + ~FunctionExpression() { delete fun; } - std::string info() const { - std::ostringstream os; - os << value; - return os.str(); - } + std::string info() const + { + std::ostringstream os; + os << "(" << fun->name << fun->params << ")"; + return os.str(); + } - void PDDLPrint( std::ostream & s, unsigned indent, const TokenStruct< std::string > & ts, const Domain & d ) const override { - s << value; - } + void PDDLPrint( + std::ostream & s, unsigned indent, const TokenStruct & ts, + const Domain & d) const override; - plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace = {} ) const override { - plansys2_msgs::msg::Node::SharedPtr node = std::make_shared(); - node->node_type = plansys2_msgs::msg::Node::NUMBER; - node->node_id = tree.nodes.size(); - node->value = value; - tree.nodes.push_back(*node); - return node; - } + plansys2_msgs::msg::Node::SharedPtr getTree( + plansys2_msgs::msg::Tree & tree, const Domain & d, + const std::vector & replace = {}) const override; - double evaluate() { return value; } + double evaluate() { return 1; } - double evaluate( Instance & ins, const StringVec & par ) { - return value; - } + double evaluate(Instance & ins, const StringVec & par); - IntSet params() { - return IntSet(); - } + IntSet params() { return IntSet(fun->params.begin(), fun->params.end()); } - Condition * copy( Domain & d ) { - return new ValueExpression( value ); - } + Condition * copy(Domain & d) + { + return new FunctionExpression(dynamic_cast(fun->copy(d))); + } }; -class DurationExpression : public Expression { +class ValueExpression : public Expression +{ +public: + double value; - void PDDLPrint( std::ostream & s, unsigned indent, const TokenStruct< std::string > & ts, const Domain & d ) const override { - s << "?duration"; - } + ValueExpression(double v) : value(v) {} - plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace = {} ) const override { - throw UnsupportedConstruct("DurationExpression"); - } + std::string info() const + { + std::ostringstream os; + os << value; + return os.str(); + } - std::string info() const { - return "?duration"; - } + void PDDLPrint( + std::ostream & s, unsigned indent, const TokenStruct & ts, + const Domain & d) const override + { + s << value; + } + + plansys2_msgs::msg::Node::SharedPtr getTree( + plansys2_msgs::msg::Tree & tree, const Domain & d, + const std::vector & replace = {}) const override + { + plansys2_msgs::msg::Node::SharedPtr node = std::make_shared(); + node->node_type = plansys2_msgs::msg::Node::NUMBER; + node->node_id = tree.nodes.size(); + node->value = value; + tree.nodes.push_back(*node); + return node; + } - double evaluate() { - return -1; - } + double evaluate() { return value; } - double evaluate( Instance & ins, const StringVec & par ) { - return evaluate(); - } + double evaluate(Instance & ins, const StringVec & par) { return value; } - IntSet params() { - return IntSet(); - } + IntSet params() { return IntSet(); } - Condition * copy( Domain & d ) { - return new DurationExpression(); - } + Condition * copy(Domain & d) { return new ValueExpression(value); } }; +class DurationExpression : public Expression +{ + void PDDLPrint( + std::ostream & s, unsigned indent, const TokenStruct & ts, + const Domain & d) const override + { + s << "?duration"; + } -class ParamExpression : public Expression { + plansys2_msgs::msg::Node::SharedPtr getTree( + plansys2_msgs::msg::Tree & tree, const Domain & d, + const std::vector & replace = {}) const override + { + throw UnsupportedConstruct("DurationExpression"); + } -public: + std::string info() const { return "?duration"; } - int param; + double evaluate() { return -1; } - ParamExpression( int p ) : param( p ) {} + double evaluate(Instance & ins, const StringVec & par) { return evaluate(); } - std::string info() const { - std::ostringstream os; - os << param; - return os.str(); - } + IntSet params() { return IntSet(); } - void PDDLPrint( std::ostream & s, unsigned indent, const TokenStruct< std::string > & ts, const Domain & d ) const override { - s << ts[param]; - } + Condition * copy(Domain & d) { return new DurationExpression(); } +}; - plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace = {} ) const override { - throw UnsupportedConstruct("ParamExpression"); - } +class ParamExpression : public Expression +{ +public: + int param; - double evaluate() { return -1; } + ParamExpression(int p) : param(p) {} - double evaluate( Instance & ins, const StringVec & par ) { - return evaluate(); - } + std::string info() const + { + std::ostringstream os; + os << param; + return os.str(); + } - IntSet params() { - return IntSet(); - } + void PDDLPrint( + std::ostream & s, unsigned indent, const TokenStruct & ts, + const Domain & d) const override + { + s << ts[param]; + } - Condition * copy( Domain & d ) { - return new ParamExpression( param ); - } -}; + plansys2_msgs::msg::Node::SharedPtr getTree( + plansys2_msgs::msg::Tree & tree, const Domain & d, + const std::vector & replace = {}) const override + { + throw UnsupportedConstruct("ParamExpression"); + } -class ConstExpression : public Expression { + double evaluate() { return -1; } -public: + double evaluate(Instance & ins, const StringVec & par) { return evaluate(); } + + IntSet params() { return IntSet(); } - int constant; - int tid; - ConstExpression( int c, int t ) : constant( c ), tid (t) {} + Condition * copy(Domain & d) { return new ParamExpression(param); } +}; - std::string info() const { - std::ostringstream os; - os << constant << " " << tid; - return os.str(); - } +class ConstExpression : public Expression +{ +public: + int constant; + int tid; + ConstExpression(int c, int t) : constant(c), tid(t) {} + + std::string info() const + { + std::ostringstream os; + os << constant << " " << tid; + return os.str(); + } - void PDDLPrint( std::ostream & s, unsigned indent, const TokenStruct< std::string > & ts, const Domain & d ) const override; + void PDDLPrint( + std::ostream & s, unsigned indent, const TokenStruct & ts, + const Domain & d) const override; - plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace = {} ) const override { - throw UnsupportedConstruct("ConstExpression"); - } + plansys2_msgs::msg::Node::SharedPtr getTree( + plansys2_msgs::msg::Tree & tree, const Domain & d, + const std::vector & replace = {}) const override + { + throw UnsupportedConstruct("ConstExpression"); + } - double evaluate() { return -1; } + double evaluate() { return -1; } - double evaluate( Instance & ins, const StringVec & par ) { - return evaluate(); - } + double evaluate(Instance & ins, const StringVec & par) { return evaluate(); } - IntSet params() { - return IntSet(); - } + IntSet params() { return IntSet(); } - Condition * copy( Domain & d ) { - return new ConstExpression( constant, tid ); - } + Condition * copy(Domain & d) { return new ConstExpression(constant, tid); } }; -} } // namespaces +} // namespace pddl +} // namespace parser diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/Forall.h b/plansys2_pddl_parser/include/plansys2_pddl_parser/Forall.h index c46b0675..07e3b578 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/Forall.h +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/Forall.h @@ -3,47 +3,49 @@ #include "plansys2_msgs/msg/node.hpp" #include "plansys2_msgs/msg/tree.hpp" - #include "plansys2_pddl_parser/ParamCond.h" -namespace parser { namespace pddl { - -class Forall : public ParamCond { - +namespace parser +{ +namespace pddl +{ +class Forall : public ParamCond +{ public: - Condition * cond; - - Forall() - : cond( 0 ) {} + Condition * cond; - Forall( const Forall * f, Domain & d ) - : ParamCond( f ), cond( 0 ) { - if ( f->cond ) cond = f->cond->copy( d ); - } + Forall() : cond(0) {} - ~Forall() { - if ( cond ) delete cond; - } + Forall(const Forall * f, Domain & d) : ParamCond(f), cond(0) + { + if (f->cond) cond = f->cond->copy(d); + } - void print( std::ostream & s ) const { - s << "Forall" << params << ":\n"; - if ( cond ) cond->print( s ); - } + ~Forall() + { + if (cond) delete cond; + } - void PDDLPrint( std::ostream & s, unsigned indent, const TokenStruct< std::string > & ts, const Domain & d ) const override; + void print(std::ostream & s) const + { + s << "Forall" << params << ":\n"; + if (cond) cond->print(s); + } - plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace = {} ) const override; + void PDDLPrint( + std::ostream & s, unsigned indent, const TokenStruct & ts, + const Domain & d) const override; - void parse( Stringreader & f, TokenStruct< std::string > & ts, Domain & d ); + plansys2_msgs::msg::Node::SharedPtr getTree( + plansys2_msgs::msg::Tree & tree, const Domain & d, + const std::vector & replace = {}) const override; - void addParams( int m, unsigned n ) { - cond->addParams( m, n ); - } + void parse(Stringreader & f, TokenStruct & ts, Domain & d); - Condition * copy( Domain & d ) { - return new Forall( this, d ); - } + void addParams(int m, unsigned n) { cond->addParams(m, n); } + Condition * copy(Domain & d) { return new Forall(this, d); } }; -} } // namespaces +} // namespace pddl +} // namespace parser diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/Function.h b/plansys2_pddl_parser/include/plansys2_pddl_parser/Function.h index b43dd28d..76ec3035 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/Function.h +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/Function.h @@ -3,32 +3,33 @@ #include "plansys2_msgs/msg/node.hpp" #include "plansys2_msgs/msg/tree.hpp" - #include "plansys2_pddl_parser/Lifted.h" -namespace parser { namespace pddl { - -class Function : public Lifted { - +namespace parser +{ +namespace pddl +{ +class Function : public Lifted +{ public: + int returnType; - int returnType; - - Function() - : Lifted(), returnType( -1 ) {} + Function() : Lifted(), returnType(-1) {} - Function( const std::string & s, int type = -1 ) - : Lifted( s ), returnType( type ) {} + Function(const std::string & s, int type = -1) : Lifted(s), returnType(type) {} - Function( const ParamCond * c ) - : Lifted( c ), returnType( -1 ) {} + Function(const ParamCond * c) : Lifted(c), returnType(-1) {} - void PDDLPrint( std::ostream & s, unsigned indent, const TokenStruct< std::string > & ts, const Domain & d ) const override; + void PDDLPrint( + std::ostream & s, unsigned indent, const TokenStruct & ts, + const Domain & d) const override; - plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace = {} ) const override; + plansys2_msgs::msg::Node::SharedPtr getTree( + plansys2_msgs::msg::Tree & tree, const Domain & d, + const std::vector & replace = {}) const override; - void parse( Stringreader & f, TokenStruct< std::string > & ts, Domain & d ); - + void parse(Stringreader & f, TokenStruct & ts, Domain & d); }; -} } // namespaces +} // namespace pddl +} // namespace parser diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/FunctionModifier.h b/plansys2_pddl_parser/include/plansys2_pddl_parser/FunctionModifier.h index bf870acf..a1e0ddd1 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/FunctionModifier.h +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/FunctionModifier.h @@ -3,94 +3,91 @@ #include "plansys2_msgs/msg/node.hpp" #include "plansys2_msgs/msg/tree.hpp" - -#include "plansys2_pddl_parser/Ground.h" #include "plansys2_pddl_parser/Condition.h" -#include "plansys2_pddl_parser/Function.h" #include "plansys2_pddl_parser/Expression.h" +#include "plansys2_pddl_parser/Function.h" +#include "plansys2_pddl_parser/Ground.h" - -namespace parser { namespace pddl { - -class FunctionModifier : public Condition { - +namespace parser +{ +namespace pddl +{ +class FunctionModifier : public Condition +{ public: + std::string name; - std::string name; + Ground * modifiedGround; // if null -> total-cost + Expression * modifierExpr; // the expression by which we increment/decrement - Ground * modifiedGround; // if null -> total-cost - Expression * modifierExpr; // the expression by which we increment/decrement + FunctionModifier(const std::string & name, int val = 1); - FunctionModifier( const std::string& name, int val = 1 ); + FunctionModifier(const std::string & name, Function * f, const IntVec & p = IntVec()); - FunctionModifier( const std::string& name, Function * f, const IntVec & p = IntVec() ); + FunctionModifier(const std::string & name, const FunctionModifier * i, Domain & d); - FunctionModifier( const std::string& name, const FunctionModifier * i, Domain & d ); + ~FunctionModifier() + { + if (modifiedGround) delete modifiedGround; + if (modifierExpr) delete modifierExpr; + } - ~FunctionModifier() { - if ( modifiedGround ) delete modifiedGround; - if ( modifierExpr ) delete modifierExpr; - } + void print(std::ostream & s) const + { + s << name << " "; + if (modifiedGround) modifiedGround->print(s); + if (modifierExpr) modifierExpr->print(s); + s << "\n"; + } - void print( std::ostream & s ) const { - s << name << " "; - if ( modifiedGround ) modifiedGround->print( s ); - if ( modifierExpr ) modifierExpr->print( s ); - s << "\n"; - } + void PDDLPrint( + std::ostream & s, unsigned indent, const TokenStruct & ts, + const Domain & d) const override; - void PDDLPrint( std::ostream & s, unsigned indent, const TokenStruct< std::string > & ts, const Domain & d ) const override; + plansys2_msgs::msg::Node::SharedPtr getTree( + plansys2_msgs::msg::Tree & tree, const Domain & d, + const std::vector & replace = {}) const override; - plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace = {} ) const override; + void parse(Stringreader & f, TokenStruct & ts, Domain & d); - void parse( Stringreader & f, TokenStruct< std::string > & ts, Domain & d ); - - void addParams( int m, unsigned n ) {} + void addParams(int m, unsigned n) {} }; -class Assign : public FunctionModifier { - +class Assign : public FunctionModifier +{ public: + Assign(int val = 1) : FunctionModifier("assign", val) {} - Assign( int val = 1 ) : FunctionModifier( "assign", val ) { } + Assign(Function * f, const IntVec & p = IntVec()) : FunctionModifier("assign", f, p) {} - Assign( Function * f, const IntVec & p = IntVec() ) : FunctionModifier( "assign", f, p ) { } + Assign(const FunctionModifier * i, Domain & d) : FunctionModifier("assign", i, d) {} - Assign( const FunctionModifier * i, Domain & d ) : FunctionModifier( "assign", i, d ) { } - - Condition * copy( Domain & d ) { - return new Assign( this, d ); - } + Condition * copy(Domain & d) { return new Assign(this, d); } }; -class Decrease : public FunctionModifier { - +class Decrease : public FunctionModifier +{ public: + Decrease(int val = 1) : FunctionModifier("decrease", val) {} - Decrease( int val = 1 ) : FunctionModifier( "decrease", val ) { } + Decrease(Function * f, const IntVec & p = IntVec()) : FunctionModifier("decrease", f, p) {} - Decrease( Function * f, const IntVec & p = IntVec() ) : FunctionModifier( "decrease", f, p ) { } + Decrease(const FunctionModifier * i, Domain & d) : FunctionModifier("decrease", i, d) {} - Decrease( const FunctionModifier * i, Domain & d ) : FunctionModifier( "decrease", i, d ) { } - - Condition * copy( Domain & d ) { - return new Decrease( this, d ); - } + Condition * copy(Domain & d) { return new Decrease(this, d); } }; -class Increase : public FunctionModifier { - +class Increase : public FunctionModifier +{ public: + Increase(int val = 1) : FunctionModifier("increase", val) {} - Increase( int val = 1 ) : FunctionModifier( "increase", val ) { } - - Increase( Function * f, const IntVec & p = IntVec() ) : FunctionModifier( "increase", f, p ) { } + Increase(Function * f, const IntVec & p = IntVec()) : FunctionModifier("increase", f, p) {} - Increase( const FunctionModifier * i, Domain & d ) : FunctionModifier( "increase", i, d ) { } + Increase(const FunctionModifier * i, Domain & d) : FunctionModifier("increase", i, d) {} - Condition * copy( Domain & d ) { - return new Increase( this, d ); - } + Condition * copy(Domain & d) { return new Increase(this, d); } }; -} } // namespaces +} // namespace pddl +} // namespace parser diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/Ground.h b/plansys2_pddl_parser/include/plansys2_pddl_parser/Ground.h index 7a4b0f06..35bf94cb 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/Ground.h +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/Ground.h @@ -3,45 +3,45 @@ #include "plansys2_msgs/msg/node.hpp" #include "plansys2_msgs/msg/tree.hpp" - #include "plansys2_pddl_parser/Lifted.h" -namespace parser { namespace pddl { - -class Ground : public ParamCond { - +namespace parser +{ +namespace pddl +{ +class Ground : public ParamCond +{ public: + Lifted * lifted; - Lifted * lifted; - - Ground() - : ParamCond(), lifted( 0 ) {} - - Ground( const std::string s, const IntVec & p = IntVec() ) - : ParamCond( s, p ), lifted( 0 ) {} + Ground() : ParamCond(), lifted(0) {} - Ground( Lifted * l, const IntVec & p = IntVec() ) - : ParamCond( l->name, p ), lifted( l ) {} + Ground(const std::string s, const IntVec & p = IntVec()) : ParamCond(s, p), lifted(0) {} - Ground( const Ground * g, Domain & d ); + Ground(Lifted * l, const IntVec & p = IntVec()) : ParamCond(l->name, p), lifted(l) {} - void PDDLPrint( std::ostream & s, unsigned indent, const TokenStruct< std::string > & ts, const Domain & d ) const override; + Ground(const Ground * g, Domain & d); - plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace = {} ) const override; + void PDDLPrint( + std::ostream & s, unsigned indent, const TokenStruct & ts, + const Domain & d) const override; - void parse( Stringreader & f, TokenStruct< std::string > & ts, Domain & d ); + plansys2_msgs::msg::Node::SharedPtr getTree( + plansys2_msgs::msg::Tree & tree, const Domain & d, + const std::vector & replace = {}) const override; - void addParams( int m, unsigned n ) { - for ( unsigned i = 0; i < params.size(); ++i ) - if ( params[i] >= m ) params[i] += n; - } + void parse(Stringreader & f, TokenStruct & ts, Domain & d); - Condition * copy( Domain & d ) { - return new Ground( this, d ); - } + void addParams(int m, unsigned n) + { + for (unsigned i = 0; i < params.size(); ++i) + if (params[i] >= m) params[i] += n; + } + Condition * copy(Domain & d) { return new Ground(this, d); } }; -typedef std::vector< Ground * > GroundVec; +typedef std::vector GroundVec; -} } // namespaces +} // namespace pddl +} // namespace parser diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/GroundFunc.h b/plansys2_pddl_parser/include/plansys2_pddl_parser/GroundFunc.h index 20beabc6..7961ea68 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/GroundFunc.h +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/GroundFunc.h @@ -3,34 +3,34 @@ #include "plansys2_msgs/msg/node.hpp" #include "plansys2_msgs/msg/tree.hpp" - #include "plansys2_pddl_parser/TypeGround.h" -namespace parser { namespace pddl { - -template < typename T > -class GroundFunc : public TypeGround { - +namespace parser +{ +namespace pddl +{ +template +class GroundFunc : public TypeGround +{ public: + T value; - T value; - - GroundFunc() - : TypeGround(), value( 0 ) {} - - GroundFunc( Lifted * l, const T & val = T( 0 ) ) - : TypeGround( l ), value( val ) {} + GroundFunc() : TypeGround(), value(0) {} - void PDDLPrint( std::ostream & s, unsigned indent, const TokenStruct< std::string > & ts, const Domain & d ) const override; + GroundFunc(Lifted * l, const T & val = T(0)) : TypeGround(l), value(val) {} - plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace = {} ) const override; + void PDDLPrint( + std::ostream & s, unsigned indent, const TokenStruct & ts, + const Domain & d) const override; - void print( std::ostream & stream ) const { - stream << name << params << value << "\n"; - } + plansys2_msgs::msg::Node::SharedPtr getTree( + plansys2_msgs::msg::Tree & tree, const Domain & d, + const std::vector & replace = {}) const override; - void parse( Stringreader & f, TokenStruct< std::string > & ts, Domain & d ); + void print(std::ostream & stream) const { stream << name << params << value << "\n"; } + void parse(Stringreader & f, TokenStruct & ts, Domain & d); }; -} } // namespaces +} // namespace pddl +} // namespace parser diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/Instance.h b/plansys2_pddl_parser/include/plansys2_pddl_parser/Instance.h index 41924a9f..4ab9aa96 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/Instance.h +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/Instance.h @@ -4,262 +4,289 @@ #include "plansys2_pddl_parser/Domain.h" // #undef DOMAIN_DEBUG // #define DOMAIN_DEBUG true -namespace parser { namespace pddl { - -class Instance { +namespace parser +{ +namespace pddl +{ +class Instance +{ public: - Domain &d; - std::string name; - GroundVec init; // initial state - Condition * goal = nullptr; // Goal condition - TokenStruct< std::string > ts; - bool metric; - - Instance( Domain & dom ) : d( dom ), metric( false ) {} - - Instance( Domain & dom, const std::string & s ) : Instance(dom) - { - parse(s); - } - - virtual ~Instance() { - for ( unsigned i = 0; i < init.size(); ++i ) - delete init[i]; - if (nullptr != goal) delete goal; - } - - void parse( const std::string &s) { - Stringreader f( s ); - name = f.parseName( "problem" ); - - if ( DOMAIN_DEBUG ) std::cout << name << "\n"; - - for ( ; f.getChar() != ')'; f.next() ) { - f.assert_token( "(" ); - f.assert_token( ":" ); - std::string t = f.getToken(); - - if ( DOMAIN_DEBUG ) std::cout << t << "\n"; - - if ( t == "domain" ) parseDomain( f ); - else if ( t == "objects" ) parseObjects( f ); - else if ( t == "init" ) parseInit( f ); - else if ( t == "goal" ) parseGoal( f ); - else if ( t == "metric" ) parseMetric( f ); - else f.tokenExit( t ); - } - } - - std::string getDomainName( const std::string &s) { - std::string domain_name = ""; - - Stringreader f( s ); - f.parseName( "problem" ); - - for ( ; f.getChar() != ')'; f.next() ) { - f.assert_token( "(" ); - f.assert_token( ":" ); - std::string t = f.getToken(); - if ( t == "domain" ) { - f.next(); - domain_name = f.getToken(); - break; - } - } - return domain_name; - } - - void parseDomain( Stringreader & f ) { - f.next(); - f.assert_token( d.name ); - f.assert_token( ")" ); - } - - void parseObjects( Stringreader & f ) { - // TokenStruct< std::string > ts = f.parseTypedList( true, d.types ); - - // We need to populate the global ts with the objects read from the - // object construct to ensure a correct reading in other places of - // the code. - ts = f.parseTypedList( true, d.types ); - - for ( unsigned i = 0; i < ts.size(); ++i ) { - Type * type = d.getType( ts.types[i] ); - std::pair< bool, unsigned > pair = type->parseObject( ts[i] ); - if ( pair.first == false ) - type->objects.insert( ts[i] ); - } - - for ( unsigned i = 0; DOMAIN_DEBUG && i < d.types.size(); ++i ) { - std::cout << " "; - if ( d.typed ) std::cout << " " << d.types[i] << ":"; - for ( unsigned j = 0; j < d.types[i]->objects.size(); ++j ) - std::cout << " " << d.types[i]->objects[j]; - std::cout << "\n"; - } - } - - - virtual void parseGround( Stringreader & f, GroundVec & v ) { - TypeGround * c = 0; - if ( f.getChar() == '=') { - f.assert_token( "=" ); - f.assert_token( "(" ); - - std::string s = f.getToken(); - int i = d.funcs.index( s ); - if ( i < 0 ) f.tokenExit( s ); - - if ( d.funcs[i]->returnType < 0 ) c = new GroundFunc< double >( d.funcs[i] ); - else c = new GroundFunc< int >( d.funcs[i] ); - } - else c = new TypeGround( d.preds.get( f.getToken( d.preds ) ) ); - c->parse( f, d.types[0]->constants, d ); - v.push_back( c ); - } - - void parseInit( Stringreader & f ) { - for ( f.next(); f.getChar() != ')'; f.next() ) { - f.assert_token( "(" ); - parseGround( f, init ); - } - ++f.c; - - for ( unsigned i = 0; DOMAIN_DEBUG && i < init.size(); ++i ) - std::cout << " " << init[i]; - } - - virtual void parseGoal( Stringreader & f ) { - f.next(); - f.assert_token( "(" ); - if ( f.getChar() != ')' ) { - goal = d.createCondition( f ); - goal->parse( f, ts, d ); + Domain & d; + std::string name; + GroundVec init; // initial state + Condition * goal = nullptr; // Goal condition + TokenStruct ts; + bool metric; + + Instance(Domain & dom) : d(dom), metric(false) {} + + Instance(Domain & dom, const std::string & s) : Instance(dom) { parse(s); } + + virtual ~Instance() + { + for (unsigned i = 0; i < init.size(); ++i) delete init[i]; + if (nullptr != goal) delete goal; } - f.next(); - f.assert_token( ")" ); - } - - // for the moment only parse total-cost/total-time - void parseMetric( Stringreader & f ) { - if ( !d.temp && !d.costs ) { - std::cerr << "metric only defined for temporal actions or actions with costs!\n"; - std::exit( 1 ); - } - - metric = true; - - f.next(); - f.assert_token( "minimize" ); - f.assert_token( "(" ); - if ( d.temp ) f.assert_token( "total-time" ); - else f.assert_token( "total-cost" ); - f.assert_token( ")" ); - f.assert_token( ")" ); - } - - // add an object of a certain type - void addObject( const std::string & name, const std::string & type ) { - // It is not enough to insert the name as an object of the given - // type. We need also to populate the TS, by insering the object - // into the ts with the below two instructions to then inform - // further calls to the parser, that there are the objects of the - // given type. - ts.insert(name); - ts.types.insert(ts.types.end(), ts.size(), type); - d.getType( type )->objects.insert( name ); - } - - // add a predicate fluent to the initial state - void addInit( const std::string & name, const StringVec & v = StringVec() ) { - TypeGround * tg = new TypeGround( d.preds.get( name ) ); - tg->insert( d, v ); - init.push_back( tg ); - } - - // add a function value to the initial state - void addInit( const std::string & name, int value, const StringVec & v = StringVec() ) { - GroundFunc< int > * gf = new GroundFunc< int >( d.funcs.get( name ), value ); - gf->insert( d, v ); - init.push_back( gf ); - } - - // add a function value to the initial state - void addInit( const std::string & name, double value, const StringVec & v = StringVec() ) { - GroundFunc< double > * gf = new GroundFunc< double >( d.funcs.get( name ), value ); - gf->insert( d, v ); - init.push_back( gf ); - } - - // add a fluent (predicate or function) to the initial state - void addInit( TypeGround * g, const StringVec & v = StringVec() ) { - TypeGround * tg = 0; - GroundFunc< int > * f1 = dynamic_cast< GroundFunc< int > * >( g ); - GroundFunc< double > * f2 = dynamic_cast< GroundFunc< double > * >( g ); - if ( f1 ) tg = new GroundFunc< int >( d.funcs.get( g->name ), f1->value ); - else if ( f2 ) tg = new GroundFunc< double >( d.funcs.get( g->name ), f2->value ); - else tg = new TypeGround( d.preds.get( g->name ) ); - tg->insert( d, v ); - init.push_back( tg ); - } - - // Add the goal represented by s to the Instance - void addGoal(const std::string & s) { - // In order to invoke the parseGoal, the input string shall also - // contain an ")" for simulating the right parenthesis of "(goal - // ....)", and an additional left ")" parenthesis for the part - // outside. We need to handle also the empty goal case - // - // Alternatively, one can define another parseGoal function - // that does not require these two additional characters. - std::string ss = ""; - if (0 == s.length()) { - ss = "(and ) ))"; - } else { - ss = s + "))"; + + void parse(const std::string & s) + { + Stringreader f(s); + name = f.parseName("problem"); + + if (DOMAIN_DEBUG) std::cout << name << "\n"; + + for (; f.getChar() != ')'; f.next()) { + f.assert_token("("); + f.assert_token(":"); + std::string t = f.getToken(); + + if (DOMAIN_DEBUG) std::cout << t << "\n"; + + if (t == "domain") + parseDomain(f); + else if (t == "objects") + parseObjects(f); + else if (t == "init") + parseInit(f); + else if (t == "goal") + parseGoal(f); + else if (t == "metric") + parseMetric(f); + else + f.tokenExit(t); + } + } + + std::string getDomainName(const std::string & s) + { + std::string domain_name = ""; + + Stringreader f(s); + f.parseName("problem"); + + for (; f.getChar() != ')'; f.next()) { + f.assert_token("("); + f.assert_token(":"); + std::string t = f.getToken(); + if (t == "domain") { + f.next(); + domain_name = f.getToken(); + break; + } + } + return domain_name; + } + + void parseDomain(Stringreader & f) + { + f.next(); + f.assert_token(d.name); + f.assert_token(")"); } - Stringreader f( ss ); - parseGoal( f ); - } - - friend std::ostream& operator<<(std::ostream &os, const Instance& o) { return o.print(os); } - virtual std::ostream& print(std::ostream& stream) const { - stream << "( define ( problem " << name << " )\n"; - stream << "( :domain " << d.name << " )\n"; - - stream << "( :objects\n"; - for ( unsigned i = 0; i < d.types.size(); ++i ) - if ( d.types[i]->objects.size() ) { - stream << "\t"; - for ( unsigned j = 0; j < d.types[i]->objects.size(); ++j ) - stream << d.types[i]->objects[j] << " "; - if ( d.typed ) stream << "- " << d.types[i]->name; - stream << "\n"; - } - stream << ")\n"; - - stream << "( :init\n"; - for ( unsigned i = 0; i < init.size(); ++i ) { - ((TypeGround*)init[i])->PDDLPrint( stream, 1, TokenStruct< std::string >(), d ); - stream << "\n"; - } - stream << ")\n"; - - stream << "( :goal\n"; - if (nullptr != goal) goal->PDDLPrint(stream, 1, ts /* TokenStruct< std::string >() */, d); - stream << ")\n"; - - if ( metric ) { - stream << "( :metric minimize ( total-"; - if ( d.temp ) stream << "time"; - else stream << "cost"; - stream << " ) )\n"; - } - - stream << ")\n"; - return stream; - } + void parseObjects(Stringreader & f) + { + // TokenStruct< std::string > ts = f.parseTypedList( true, d.types ); + + // We need to populate the global ts with the objects read from the + // object construct to ensure a correct reading in other places of + // the code. + ts = f.parseTypedList(true, d.types); + + for (unsigned i = 0; i < ts.size(); ++i) { + Type * type = d.getType(ts.types[i]); + std::pair pair = type->parseObject(ts[i]); + if (pair.first == false) type->objects.insert(ts[i]); + } + + for (unsigned i = 0; DOMAIN_DEBUG && i < d.types.size(); ++i) { + std::cout << " "; + if (d.typed) std::cout << " " << d.types[i] << ":"; + for (unsigned j = 0; j < d.types[i]->objects.size(); ++j) + std::cout << " " << d.types[i]->objects[j]; + std::cout << "\n"; + } + } + + virtual void parseGround(Stringreader & f, GroundVec & v) + { + TypeGround * c = 0; + if (f.getChar() == '=') { + f.assert_token("="); + f.assert_token("("); + + std::string s = f.getToken(); + int i = d.funcs.index(s); + if (i < 0) f.tokenExit(s); + + if (d.funcs[i]->returnType < 0) + c = new GroundFunc(d.funcs[i]); + else + c = new GroundFunc(d.funcs[i]); + } else + c = new TypeGround(d.preds.get(f.getToken(d.preds))); + c->parse(f, d.types[0]->constants, d); + v.push_back(c); + } + + void parseInit(Stringreader & f) + { + for (f.next(); f.getChar() != ')'; f.next()) { + f.assert_token("("); + parseGround(f, init); + } + ++f.c; + + for (unsigned i = 0; DOMAIN_DEBUG && i < init.size(); ++i) std::cout << " " << init[i]; + } + + virtual void parseGoal(Stringreader & f) + { + f.next(); + f.assert_token("("); + if (f.getChar() != ')') { + goal = d.createCondition(f); + goal->parse(f, ts, d); + } + f.next(); + f.assert_token(")"); + } + + // for the moment only parse total-cost/total-time + void parseMetric(Stringreader & f) + { + if (!d.temp && !d.costs) { + std::cerr << "metric only defined for temporal actions or actions with costs!\n"; + std::exit(1); + } + + metric = true; + + f.next(); + f.assert_token("minimize"); + f.assert_token("("); + if (d.temp) + f.assert_token("total-time"); + else + f.assert_token("total-cost"); + f.assert_token(")"); + f.assert_token(")"); + } + + // add an object of a certain type + void addObject(const std::string & name, const std::string & type) + { + // It is not enough to insert the name as an object of the given + // type. We need also to populate the TS, by inserting the object + // into the ts with the below two instructions to then inform + // further calls to the parser, that there are the objects of the + // given type. + ts.insert(name); + ts.types.insert(ts.types.end(), ts.size(), type); + d.getType(type)->objects.insert(name); + } + + // add a predicate fluent to the initial state + void addInit(const std::string & name, const StringVec & v = StringVec()) + { + TypeGround * tg = new TypeGround(d.preds.get(name)); + tg->insert(d, v); + init.push_back(tg); + } + + // add a function value to the initial state + void addInit(const std::string & name, int value, const StringVec & v = StringVec()) + { + GroundFunc * gf = new GroundFunc(d.funcs.get(name), value); + gf->insert(d, v); + init.push_back(gf); + } + + // add a function value to the initial state + void addInit(const std::string & name, double value, const StringVec & v = StringVec()) + { + GroundFunc * gf = new GroundFunc(d.funcs.get(name), value); + gf->insert(d, v); + init.push_back(gf); + } + + // add a fluent (predicate or function) to the initial state + void addInit(TypeGround * g, const StringVec & v = StringVec()) + { + TypeGround * tg = 0; + GroundFunc * f1 = dynamic_cast *>(g); + GroundFunc * f2 = dynamic_cast *>(g); + if (f1) + tg = new GroundFunc(d.funcs.get(g->name), f1->value); + else if (f2) + tg = new GroundFunc(d.funcs.get(g->name), f2->value); + else + tg = new TypeGround(d.preds.get(g->name)); + tg->insert(d, v); + init.push_back(tg); + } + + // Add the goal represented by s to the Instance + void addGoal(const std::string & s) + { + // In order to invoke the parseGoal, the input string shall also + // contain an ")" for simulating the right parenthesis of "(goal + // ....)", and an additional left ")" parenthesis for the part + // outside. We need to handle also the empty goal case + // + // Alternatively, one can define another parseGoal function + // that does not require these two additional characters. + std::string ss = ""; + if (0 == s.length()) { + ss = "(and ) ))"; + } else { + ss = s + "))"; + } + Stringreader f(ss); + parseGoal(f); + } + + friend std::ostream & operator<<(std::ostream & os, const Instance & o) { return o.print(os); } + virtual std::ostream & print(std::ostream & stream) const + { + stream << "( define ( problem " << name << " )\n"; + stream << "( :domain " << d.name << " )\n"; + + stream << "( :objects\n"; + for (unsigned i = 0; i < d.types.size(); ++i) + if (d.types[i]->objects.size()) { + stream << "\t"; + for (unsigned j = 0; j < d.types[i]->objects.size(); ++j) + stream << d.types[i]->objects[j] << " "; + if (d.typed) stream << "- " << d.types[i]->name; + stream << "\n"; + } + stream << ")\n"; + + stream << "( :init\n"; + for (unsigned i = 0; i < init.size(); ++i) { + ((TypeGround *)init[i])->PDDLPrint(stream, 1, TokenStruct(), d); + stream << "\n"; + } + stream << ")\n"; + + stream << "( :goal\n"; + if (nullptr != goal) goal->PDDLPrint(stream, 1, ts /* TokenStruct< std::string >() */, d); + stream << ")\n"; + + if (metric) { + stream << "( :metric minimize ( total-"; + if (d.temp) + stream << "time"; + else + stream << "cost"; + stream << " ) )\n"; + } + + stream << ")\n"; + return stream; + } }; -} } // namespaces +} // namespace pddl +} // namespace parser diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/Lifted.h b/plansys2_pddl_parser/include/plansys2_pddl_parser/Lifted.h index cd55ee3c..b769f8cd 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/Lifted.h +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/Lifted.h @@ -3,37 +3,37 @@ #include "plansys2_msgs/msg/node.hpp" #include "plansys2_msgs/msg/tree.hpp" - #include "plansys2_pddl_parser/ParamCond.h" -namespace parser { namespace pddl { - -class Lifted : public ParamCond { - +namespace parser +{ +namespace pddl +{ +class Lifted : public ParamCond +{ public: + Lifted() {} - Lifted() {} - - Lifted( const std::string & s ) - : ParamCond( s ) {} - - Lifted( const ParamCond * c ) - : ParamCond( c ) {} + Lifted(const std::string & s) : ParamCond(s) {} - void PDDLPrint( std::ostream & s, unsigned indent, const TokenStruct< std::string > & ts, const Domain & d ) const override; + Lifted(const ParamCond * c) : ParamCond(c) {} - plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace = {} ) const override; + void PDDLPrint( + std::ostream & s, unsigned indent, const TokenStruct & ts, + const Domain & d) const override; - void parse( Stringreader & f, TokenStruct< std::string > & ts, Domain & d ); + plansys2_msgs::msg::Node::SharedPtr getTree( + plansys2_msgs::msg::Tree & tree, const Domain & d, + const std::vector & replace = {}) const override; - void addParams( int m, unsigned n ) {} + void parse(Stringreader & f, TokenStruct & ts, Domain & d); - Condition * copy( Domain & d ) { - return new Lifted( this ); - } + void addParams(int m, unsigned n) {} + Condition * copy(Domain & d) { return new Lifted(this); } }; -typedef std::vector< Lifted * > LiftedVec; +typedef std::vector LiftedVec; -} } // namespaces +} // namespace pddl +} // namespace parser diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/Not.h b/plansys2_pddl_parser/include/plansys2_pddl_parser/Not.h index 4fba6c6b..86fdb8ff 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/Not.h +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/Not.h @@ -3,51 +3,51 @@ #include "plansys2_msgs/msg/node.hpp" #include "plansys2_msgs/msg/tree.hpp" - #include "plansys2_pddl_parser/Ground.h" -namespace parser { namespace pddl { - -class Not : public Condition { - +namespace parser +{ +namespace pddl +{ +class Not : public Condition +{ public: + Ground * cond; - Ground * cond; - - Not() - : cond( 0 ) {} - - Not( Ground * g ) - : cond( g ) {} + Not() : cond(0) {} - Not( const Not * n, Domain & d ) - : cond( 0 ) { - if ( n->cond ) cond = ( Ground * )n->cond->copy( d ); - } + Not(Ground * g) : cond(g) {} - ~Not() { - if ( cond ) delete cond; - } + Not(const Not * n, Domain & d) : cond(0) + { + if (n->cond) cond = (Ground *)n->cond->copy(d); + } - void print( std::ostream & s ) const { - s << "not "; - if ( cond ) cond->print( s ); - } + ~Not() + { + if (cond) delete cond; + } - void PDDLPrint( std::ostream & s, unsigned indent, const TokenStruct< std::string > & ts, const Domain & d ) const override; + void print(std::ostream & s) const + { + s << "not "; + if (cond) cond->print(s); + } - plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace = {} ) const override; + void PDDLPrint( + std::ostream & s, unsigned indent, const TokenStruct & ts, + const Domain & d) const override; - void parse( Stringreader & f, TokenStruct< std::string > & ts, Domain & d ); + plansys2_msgs::msg::Node::SharedPtr getTree( + plansys2_msgs::msg::Tree & tree, const Domain & d, + const std::vector & replace = {}) const override; - void addParams( int m, unsigned n ) { - cond->addParams( m, n ); - } + void parse(Stringreader & f, TokenStruct & ts, Domain & d); - Condition * copy( Domain & d ) { - return new Not( this, d ); - } + void addParams(int m, unsigned n) { cond->addParams(m, n); } + Condition * copy(Domain & d) { return new Not(this, d); } }; -} } // namespaces +} // namespace pddl +} // namespace parser diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/Oneof.h b/plansys2_pddl_parser/include/plansys2_pddl_parser/Oneof.h index 69852e81..2210c5af 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/Oneof.h +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/Oneof.h @@ -3,53 +3,53 @@ #include "plansys2_msgs/msg/node.hpp" #include "plansys2_msgs/msg/tree.hpp" - #include "plansys2_pddl_parser/Condition.h" -namespace parser { namespace pddl { - -class Oneof : public Condition { - +namespace parser +{ +namespace pddl +{ +class Oneof : public Condition +{ public: + CondVec conds; - CondVec conds; - - Oneof() {} - - Oneof( const Oneof * o, Domain & d ) { - for ( unsigned i = 0; i < o->conds.size(); ++i ) - conds.push_back( o->conds[i]->copy( d ) ); - } + Oneof() {} - ~Oneof() { - for ( unsigned i = 0; i < conds.size(); ++i ) - delete conds[i]; - } + Oneof(const Oneof * o, Domain & d) + { + for (unsigned i = 0; i < o->conds.size(); ++i) conds.push_back(o->conds[i]->copy(d)); + } - void print( std::ostream & s ) const { - for ( unsigned i = 0; i < conds.size(); ++i ) - conds[i]->print( s ); - } + ~Oneof() + { + for (unsigned i = 0; i < conds.size(); ++i) delete conds[i]; + } - void PDDLPrint( std::ostream & s, unsigned indent, const TokenStruct< std::string > & ts, const Domain & d ) const override; + void print(std::ostream & s) const + { + for (unsigned i = 0; i < conds.size(); ++i) conds[i]->print(s); + } - plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace = {} ) const override; + void PDDLPrint( + std::ostream & s, unsigned indent, const TokenStruct & ts, + const Domain & d) const override; - void parse( Stringreader & f, TokenStruct< std::string > & ts, Domain & d ); + plansys2_msgs::msg::Node::SharedPtr getTree( + plansys2_msgs::msg::Tree & tree, const Domain & d, + const std::vector & replace = {}) const override; - void add( Condition * cond ) { - conds.push_back( cond ); - } + void parse(Stringreader & f, TokenStruct & ts, Domain & d); - void addParams( int m, unsigned n ) { - for ( unsigned i = 0; i < conds.size(); ++i ) - conds[i]->addParams( m, n ); - } + void add(Condition * cond) { conds.push_back(cond); } - Condition * copy( Domain & d ) { - return new Oneof( this, d ); - } + void addParams(int m, unsigned n) + { + for (unsigned i = 0; i < conds.size(); ++i) conds[i]->addParams(m, n); + } + Condition * copy(Domain & d) { return new Oneof(this, d); } }; -} } // namespaces +} // namespace pddl +} // namespace parser diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/Or.h b/plansys2_pddl_parser/include/plansys2_pddl_parser/Or.h index 39095932..9f088445 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/Or.h +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/Or.h @@ -3,50 +3,56 @@ #include "plansys2_msgs/msg/node.hpp" #include "plansys2_msgs/msg/tree.hpp" - #include "plansys2_pddl_parser/Condition.h" -namespace parser { namespace pddl { - -class Or : public Condition { - +namespace parser +{ +namespace pddl +{ +class Or : public Condition +{ public: - Condition *first, *second; - - Or() - : first( 0 ), second( 0 ) {} - - Or( const Or * o, Domain & d ) - : first( 0 ), second( 0 ) { - if ( o->first ) first = o->first->copy( d ); - if ( o->second ) second = o->second->copy( d ); - } - - ~Or() { - if ( first ) delete first; - if ( second ) delete second; - } - - void print( std::ostream & s ) const { - s << "OR:\n"; - if ( first ) first->print( s ); - if ( second ) second->print( s ); - } - - void PDDLPrint( std::ostream & s, unsigned indent, const TokenStruct< std::string > & ts, const Domain & d ) const override; - - plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace = {} ) const override; - - void parse( Stringreader & f, TokenStruct< std::string > & ts, Domain & d ); - - void addParams( int m, unsigned n ) { - first->addParams( m, n ); - second->addParams( m, n ); - } - - Condition * copy( Domain & d ) { - return new Or( this, d ); - } + Condition *first, *second; + + Or() : first(0), second(0) {} + + Or(const Or * o, Domain & d) : first(0), second(0) + { + if (o->first) first = o->first->copy(d); + if (o->second) second = o->second->copy(d); + } + + ~Or() + { + if (first) delete first; + if (second) delete second; + } + + void print(std::ostream & s) const + { + s << "OR:\n"; + if (first) first->print(s); + if (second) second->print(s); + } + + void PDDLPrint( + std::ostream & s, unsigned indent, const TokenStruct & ts, + const Domain & d) const override; + + plansys2_msgs::msg::Node::SharedPtr getTree( + plansys2_msgs::msg::Tree & tree, const Domain & d, + const std::vector & replace = {}) const override; + + void parse(Stringreader & f, TokenStruct & ts, Domain & d); + + void addParams(int m, unsigned n) + { + first->addParams(m, n); + second->addParams(m, n); + } + + Condition * copy(Domain & d) { return new Or(this, d); } }; -} } // namespaces +} // namespace pddl +} // namespace parser diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/ParamCond.h b/plansys2_pddl_parser/include/plansys2_pddl_parser/ParamCond.h index 5c5448a3..d22cdae9 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/ParamCond.h +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/ParamCond.h @@ -1,40 +1,37 @@ #pragma once -#include "plansys2_pddl_parser/Condition.h" #include "plansys2_pddl_parser/Basic.h" +#include "plansys2_pddl_parser/Condition.h" -namespace parser { namespace pddl { - - +namespace parser +{ +namespace pddl +{ // This is necessary for adl using ::operator<<; -class ParamCond : public Condition { - +class ParamCond : public Condition +{ public: - std::string name; - IntVec params; + std::string name; + IntVec params; - ParamCond() {} + ParamCond() {} - ParamCond( const std::string & s, const IntVec & p = IntVec() ) - : name( s ), params( p ) {} + ParamCond(const std::string & s, const IntVec & p = IntVec()) : name(s), params(p) {} - ParamCond( const ParamCond * c ) - : name( c->name ), params( c->params ) {} + ParamCond(const ParamCond * c) : name(c->name), params(c->params) {} - std::string c_str() const { - return name; - } + std::string c_str() const { return name; } - void print( std::ostream & stream ) const { - stream << name << params << "\n"; - } + void print(std::ostream & stream) const { stream << name << params << "\n"; } - void printParams( unsigned first, std::ostream & s, TokenStruct< std::string > & ts, const Domain & d ) const; + void printParams( + unsigned first, std::ostream & s, TokenStruct & ts, const Domain & d) const; }; -typedef std::vector< ParamCond * > ParamCondVec; +typedef std::vector ParamCondVec; -} } // namespaces +} // namespace pddl +} // namespace parser diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/Stringreader.h b/plansys2_pddl_parser/include/plansys2_pddl_parser/Stringreader.h index 8ab44028..a7a5415c 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/Stringreader.h +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/Stringreader.h @@ -8,198 +8,200 @@ #include "plansys2_pddl_parser/TokenStruct.h" #include "plansys2_pddl_parser/Type.h" -namespace parser { namespace pddl { - - - +namespace parser +{ +namespace pddl +{ class Domain; -class ExpectedToken : public std::runtime_error { +class ExpectedToken : public std::runtime_error +{ public: - ExpectedToken(const std::string& token) : std::runtime_error(token + " expected") {} + ExpectedToken(const std::string & token) : std::runtime_error(token + " expected") {} }; -class UnknownToken : public std::runtime_error { +class UnknownToken : public std::runtime_error +{ public: - UnknownToken(const std::string& token) : std::runtime_error(token + " does not name a known token") {} + UnknownToken(const std::string & token) + : std::runtime_error(token + " does not name a known token") + { + } }; -class UnexpectedEOF : public std::runtime_error { +class UnexpectedEOF : public std::runtime_error +{ public: - UnexpectedEOF() : std::runtime_error("Unexpected EOF found") {} + UnexpectedEOF() : std::runtime_error("Unexpected EOF found") {} }; -class Stringreader { - +class Stringreader +{ public: - - std::vector lines; - int current_line; // current line of file - std::string s; - unsigned r, c; // current row and column of file - - Stringreader( const std::string & domain ) : current_line( 0 ), r( 1 ), c( 0 ) { - - lines = getLines(domain); - - s = lines[current_line++]; - std::transform(s.begin(), s.end(),s.begin(), ::tolower); - next(); - } - - ~Stringreader() { - } - - std::vector getLines(const std::string & text) - { - std::vector ret; - size_t start = 0, end = 0; - - while (end != std::string::npos) { - end = text.find("\n", start); - ret.push_back(text.substr(start, (end == std::string::npos) ? std::string::npos : end - start)); - start = ((end > (std::string::npos - 1)) ? std::string::npos : end + 1); - } - - return ret; - } - - - // characters to be ignored - bool ignore( char c ) { - return c == ' ' || c == '\t' || c == '\r' || c == '\n' || c == '\f'; - } - - // parenthesis - bool paren( char c ) { - return c == '(' || c == ')' || c == '{' || c == '}'; - } - - // current character - char getChar() { - return s[c]; - } - - // print line and column - void printLine() { - std::cout << "Line " << r << ", column " << c+1 << ": "; - } - - void tokenExit( const std::string & t ) { - c -= t.size(); - printLine(); - throw UnknownToken(t); - } - - // get next non-ignored character - void next() { - for ( ; c < s.size() && ignore( s[c] ); ++c ); - while ( c == s.size() || s[c] == ';' ) { - - - ++r; - c = 0; - - s = lines[current_line++]; - std::transform(s.begin(), s.end(),s.begin(), ::tolower); - - for ( ; c < s.size() && ignore( s[c] ); ++c ); - } - } - - // get token converted to lowercase - std::string getToken() { - std::ostringstream os; - while ( c < s.size() && !ignore( s[c] ) && !paren( s[c] ) && s[c] != ',' ) - os << ( 65 <= s[c] && s[c] <= 90 ? (char)( s[c++] + 32 ) : s[c++] ); - - return os.str(); - } - - // get token converted to lowercase - // check that the token exists - template < typename T > - std::string getToken( const TokenStruct< T > & ts ) { - std::string t = getToken(); - if ( ts.index( t ) < 0 ) - tokenExit( t ); - return t; - } - - // assert syntax - void assert_token( const std::string & t ) { - unsigned b = 0; - for ( unsigned k = 0; c + k < s.size() && k < t.size(); ++k ) - b += s[c + k] == t[k] || - ( 65 <= s[c + k] && s[c + k] <= 90 && s[c + k] == t[k] - 32 ); - - if ( b < t.size() ) { - printLine(); - throw ExpectedToken(t); - } - c += t.size(); - next(); - } - - // parse the name of a domain or instance - std::string parseName( const std::string & u ) { - std::string out; - std::string t[5] = { "(", "define", "(", u, ")" }; - for ( unsigned i = 0; i < 5; ++i ) { - assert_token( t[i] ); - if ( i == 3 ) { - out = getToken(); - next(); - } - } - return out; - } - - - // parse a typed list - // if check is true, checks that types exist - TokenStruct< std::string > parseTypedList( bool check, const TokenStruct< Type * > & ts = TokenStruct< Type * >(), const std::string & lt = "" ) { - unsigned k = 0; - TokenStruct< std::string > out; - for ( next(); getChar() != ')' && lt.find( getChar() ) == std::string::npos; next() ) { - if ( getChar() == '-' ) { - assert_token( "-" ); - - std::string t; - // check if the type is "either" - if ( getChar() == '(' ) { - assert_token( "(" ); - assert_token( "either" ); - - t = "( either"; - for ( ; getChar() != ')'; next() ) { - if ( check ) t += " " + getToken( ts ); - else t += " " + getToken(); - } - t += " )"; - ++c; - } - else if ( check ) t = getToken( ts ); - else t = getToken(); - - out.types.insert( out.types.end(), out.size() - k, t ); - k = out.size(); - } - else if ( getChar() == '(' ) { - assert_token( "(" ); - assert_token( ":private" ); - getToken(); - out.append( parseTypedList( check, ts ) ); - } - else out.insert( getToken() ); - } - if ( k < out.size() ) out.types.insert( out.types.end(), out.size() - k, check ? "object" : "" ); - ++c; - - return out; - } - + std::vector lines; + int current_line; // current line of file + std::string s; + unsigned r, c; // current row and column of file + + Stringreader(const std::string & domain) : current_line(0), r(1), c(0) + { + lines = getLines(domain); + + s = lines[current_line++]; + std::transform(s.begin(), s.end(), s.begin(), ::tolower); + next(); + } + + ~Stringreader() {} + + std::vector getLines(const std::string & text) + { + std::vector ret; + size_t start = 0, end = 0; + + while (end != std::string::npos) { + end = text.find("\n", start); + ret.push_back( + text.substr(start, (end == std::string::npos) ? std::string::npos : end - start)); + start = ((end > (std::string::npos - 1)) ? std::string::npos : end + 1); + } + + return ret; + } + + // characters to be ignored + bool ignore(char c) { return c == ' ' || c == '\t' || c == '\r' || c == '\n' || c == '\f'; } + + // parenthesis + bool paren(char c) { return c == '(' || c == ')' || c == '{' || c == '}'; } + + // current character + char getChar() { return s[c]; } + + // print line and column + void printLine() { std::cout << "Line " << r << ", column " << c + 1 << ": "; } + + void tokenExit(const std::string & t) + { + c -= t.size(); + printLine(); + throw UnknownToken(t); + } + + // get next non-ignored character + void next() + { + for (; c < s.size() && ignore(s[c]); ++c) + ; + while (c == s.size() || s[c] == ';') { + ++r; + c = 0; + + s = lines[current_line++]; + std::transform(s.begin(), s.end(), s.begin(), ::tolower); + + for (; c < s.size() && ignore(s[c]); ++c) + ; + } + } + + // get token converted to lowercase + std::string getToken() + { + std::ostringstream os; + while (c < s.size() && !ignore(s[c]) && !paren(s[c]) && s[c] != ',') + os << (65 <= s[c] && s[c] <= 90 ? (char)(s[c++] + 32) : s[c++]); + + return os.str(); + } + + // get token converted to lowercase + // check that the token exists + template + std::string getToken(const TokenStruct & ts) + { + std::string t = getToken(); + if (ts.index(t) < 0) tokenExit(t); + return t; + } + + // assert syntax + void assert_token(const std::string & t) + { + unsigned b = 0; + for (unsigned k = 0; c + k < s.size() && k < t.size(); ++k) + b += s[c + k] == t[k] || (65 <= s[c + k] && s[c + k] <= 90 && s[c + k] == t[k] - 32); + + if (b < t.size()) { + printLine(); + throw ExpectedToken(t); + } + c += t.size(); + next(); + } + + // parse the name of a domain or instance + std::string parseName(const std::string & u) + { + std::string out; + std::string t[5] = {"(", "define", "(", u, ")"}; + for (unsigned i = 0; i < 5; ++i) { + assert_token(t[i]); + if (i == 3) { + out = getToken(); + next(); + } + } + return out; + } + + // parse a typed list + // if check is true, checks that types exist + TokenStruct parseTypedList( + bool check, const TokenStruct & ts = TokenStruct(), const std::string & lt = "") + { + unsigned k = 0; + TokenStruct out; + for (next(); getChar() != ')' && lt.find(getChar()) == std::string::npos; next()) { + if (getChar() == '-') { + assert_token("-"); + + std::string t; + // check if the type is "either" + if (getChar() == '(') { + assert_token("("); + assert_token("either"); + + t = "( either"; + for (; getChar() != ')'; next()) { + if (check) + t += " " + getToken(ts); + else + t += " " + getToken(); + } + t += " )"; + ++c; + } else if (check) + t = getToken(ts); + else + t = getToken(); + + out.types.insert(out.types.end(), out.size() - k, t); + k = out.size(); + } else if (getChar() == '(') { + assert_token("("); + assert_token(":private"); + getToken(); + out.append(parseTypedList(check, ts)); + } else + out.insert(getToken()); + } + if (k < out.size()) out.types.insert(out.types.end(), out.size() - k, check ? "object" : ""); + ++c; + + return out; + } }; -} } // namespaces - +} // namespace pddl +} // namespace parser diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/Task.h b/plansys2_pddl_parser/include/plansys2_pddl_parser/Task.h index 2c0f0546..c8f9cea6 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/Task.h +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/Task.h @@ -3,39 +3,42 @@ #include "plansys2_msgs/msg/node.hpp" #include "plansys2_msgs/msg/tree.hpp" - #include "plansys2_pddl_parser/ParamCond.h" -namespace parser { namespace pddl { - -class Task : public ParamCond { - +namespace parser +{ +namespace pddl +{ +class Task : public ParamCond +{ public: + Task() {} - Task() {} - - Task( const std::string & s ) - : ParamCond( s ) {} - - Task( const ParamCond * c ) - : ParamCond( c ) {} + Task(const std::string & s) : ParamCond(s) {} - void PDDLPrint( std::ostream & s, unsigned indent, const TokenStruct< std::string > & ts, const Domain & d ) const override {} + Task(const ParamCond * c) : ParamCond(c) {} - plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace = {} ) const override { - throw UnsupportedConstruct("Task"); - } + void PDDLPrint( + std::ostream & s, unsigned indent, const TokenStruct & ts, + const Domain & d) const override + { + } - void parse( Stringreader & f, TokenStruct< std::string > & ts, Domain & d ) {} + plansys2_msgs::msg::Node::SharedPtr getTree( + plansys2_msgs::msg::Tree & tree, const Domain & d, + const std::vector & replace = {}) const override + { + throw UnsupportedConstruct("Task"); + } - void addParams( int m, unsigned n ) {} + void parse(Stringreader & f, TokenStruct & ts, Domain & d) {} - Condition * copy( Domain & d ) { - return new Task( this ); - } + void addParams(int m, unsigned n) {} + Condition * copy(Domain & d) { return new Task(this); } }; -typedef std::vector< Task * > TaskVec; +typedef std::vector TaskVec; -} } // namespaces +} // namespace pddl +} // namespace parser diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/TemporalAction.h b/plansys2_pddl_parser/include/plansys2_pddl_parser/TemporalAction.h index 8c929ac2..82e2e8ab 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/TemporalAction.h +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/TemporalAction.h @@ -3,72 +3,84 @@ #include "plansys2_msgs/msg/node.hpp" #include "plansys2_msgs/msg/tree.hpp" - #include "plansys2_pddl_parser/Action.h" #include "plansys2_pddl_parser/And.h" #include "plansys2_pddl_parser/Expression.h" -namespace parser { namespace pddl { - +namespace parser +{ +namespace pddl +{ class Instance; -class TemporalAction : public Action { - +class TemporalAction : public Action +{ public: + Expression * durationExpr; + // pre_s and eff_s inherited from Action + And *pre_o, *pre_e, *eff_e; - Expression * durationExpr; - // pre_s and eff_s inherited from Action - And *pre_o, *pre_e, *eff_e; - - TemporalAction( const std::string & s ) - : Action( s ), durationExpr( 0 ), pre_o( 0 ), pre_e( 0 ), eff_e( 0 ) {} + TemporalAction(const std::string & s) : Action(s), durationExpr(0), pre_o(0), pre_e(0), eff_e(0) + { + } - ~TemporalAction() { - if ( durationExpr ) delete durationExpr; - if ( pre_o ) delete pre_o; - if ( pre_e ) delete pre_e; - if ( eff_e ) delete eff_e; - } + ~TemporalAction() + { + if (durationExpr) delete durationExpr; + if (pre_o) delete pre_o; + if (pre_e) delete pre_e; + if (eff_e) delete eff_e; + } - void print( std::ostream & s ) const { - s << name << params << "\n"; - s << "Duration: " << durationExpr->info() << "\n"; - s << "Pre_s: " << pre; - s << "Eff_s: " << eff; - s << "Pre_o: " << pre_o; - s << "Pre_e: " << pre_e; - s << "Eff_e: " << eff_e; - } + void print(std::ostream & s) const + { + s << name << params << "\n"; + s << "Duration: " << durationExpr->info() << "\n"; + s << "Pre_s: " << pre; + s << "Eff_s: " << eff; + s << "Pre_o: " << pre_o; + s << "Pre_e: " << pre_e; + s << "Eff_e: " << eff_e; + } - double duration() { - if ( durationExpr ) return durationExpr->evaluate(); - else return 0; - } + double duration() + { + if (durationExpr) + return durationExpr->evaluate(); + else + return 0; + } - Expression * parseDuration( Stringreader & f, TokenStruct< std::string > & ts, Domain & d ); + Expression * parseDuration(Stringreader & f, TokenStruct & ts, Domain & d); - void printCondition( std::ostream & s, const TokenStruct< std::string > & ts, const Domain & d, - const std::string & t, And * a ) const; + void printCondition( + std::ostream & s, const TokenStruct & ts, const Domain & d, const std::string & t, + And * a) const; - void PDDLPrint( std::ostream & s, unsigned indent, const TokenStruct< std::string > & ts, const Domain & d ) const override; + void PDDLPrint( + std::ostream & s, unsigned indent, const TokenStruct & ts, + const Domain & d) const override; - plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace = {} ) const override; + plansys2_msgs::msg::Node::SharedPtr getTree( + plansys2_msgs::msg::Tree & tree, const Domain & d, + const std::vector & replace = {}) const override; - void parseCondition( Stringreader & f, TokenStruct< std::string > & ts, Domain & d, And * a ); + void parseCondition(Stringreader & f, TokenStruct & ts, Domain & d, And * a); - void parse( Stringreader & f, TokenStruct< std::string > & ts, Domain & d ); + void parse(Stringreader & f, TokenStruct & ts, Domain & d); - GroundVec preconsStart(); + GroundVec preconsStart(); - GroundVec preconsOverall(); + GroundVec preconsOverall(); - GroundVec preconsEnd(); + GroundVec preconsEnd(); - CondVec endEffects(); + CondVec endEffects(); - GroundVec addEndEffects(); + GroundVec addEndEffects(); - GroundVec deleteEndEffects(); + GroundVec deleteEndEffects(); }; -} } // namespaces +} // namespace pddl +} // namespace parser diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/TokenStruct.h b/plansys2_pddl_parser/include/plansys2_pddl_parser/TokenStruct.h index 0a54ff25..783f4654 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/TokenStruct.h +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/TokenStruct.h @@ -1,83 +1,80 @@ #pragma once #include + #include "plansys2_pddl_parser/Basic.h" /* - T is either a pointer or a string - A token structure stores pointers but does not delete them! + T is either a pointer or a string + A token structure stores pointers but does not delete them! */ -namespace parser { namespace pddl { - -typedef std::map< std::string, unsigned > TokenMap; +namespace parser +{ +namespace pddl +{ +typedef std::map TokenMap; template -inline std::string getName( const T & t ) { - return t->name; +inline std::string getName(const T & t) +{ + return t->name; } template <> -inline std::string getName( const std::string & s ) { - return s; +inline std::string getName(const std::string & s) +{ + return s; } template -class TokenStruct { - +class TokenStruct +{ public: + std::vector tokens; + TokenMap tokenMap; - std::vector< T > tokens; - TokenMap tokenMap; - - // represents the types of a typed list - StringVec types; - - TokenStruct() {} + // represents the types of a typed list + StringVec types; - TokenStruct( const TokenStruct & ts ) - : tokens( ts.tokens ), tokenMap( ts.tokenMap ), types( ts.types ) {} + TokenStruct() {} - void append( const TokenStruct & ts ) { - for ( unsigned i = 0; i < ts.size(); ++i ) - insert( ts[i] ); - types.insert( types.end(), ts.types.begin(), ts.types.end() ); - } + TokenStruct(const TokenStruct & ts) : tokens(ts.tokens), tokenMap(ts.tokenMap), types(ts.types) {} - unsigned size() const { - return tokens.size(); - } + void append(const TokenStruct & ts) + { + for (unsigned i = 0; i < ts.size(); ++i) insert(ts[i]); + types.insert(types.end(), ts.types.begin(), ts.types.end()); + } - T & operator[]( std::size_t i ) { - return tokens[i]; - } + unsigned size() const { return tokens.size(); } - const T & operator[]( std::size_t i ) const { - return tokens[i]; - } + T & operator[](std::size_t i) { return tokens[i]; } - void clear() { - for ( unsigned i = 0; i < size(); ++i ) - delete tokens[i]; + const T & operator[](std::size_t i) const { return tokens[i]; } - tokens.clear(); - tokenMap.clear(); - } + void clear() + { + for (unsigned i = 0; i < size(); ++i) delete tokens[i]; - unsigned insert( const T & t ) { - TokenMap::iterator i = tokenMap.insert( tokenMap.begin(), std::make_pair( getName( t ), size() ) ); - tokens.push_back( t ); - return i->second; - } + tokens.clear(); + tokenMap.clear(); + } - int index( const std::string & s ) const { - TokenMap::const_iterator i = tokenMap.find( s ); - return i == tokenMap.end() ? -1 : i->second; - } + unsigned insert(const T & t) + { + TokenMap::iterator i = tokenMap.insert(tokenMap.begin(), std::make_pair(getName(t), size())); + tokens.push_back(t); + return i->second; + } - T get( const std::string & s ) const { - return tokens[index( s )]; - } + int index(const std::string & s) const + { + TokenMap::const_iterator i = tokenMap.find(s); + return i == tokenMap.end() ? -1 : i->second; + } + T get(const std::string & s) const { return tokens[index(s)]; } }; -} } // namespaces +} // namespace pddl +} // namespace parser diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/Type.h b/plansys2_pddl_parser/include/plansys2_pddl_parser/Type.h index d9988573..17e0402b 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/Type.h +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/Type.h @@ -3,142 +3,160 @@ #include "plansys2_pddl_parser/TokenStruct.h" -namespace parser { namespace pddl { - +namespace parser +{ +namespace pddl +{ class Type; -typedef std::vector< Type * > TypeVec; - -class Type { +typedef std::vector TypeVec; +class Type +{ public: - - std::string name; - TypeVec subtypes; - Type * supertype; - - TokenStruct< std::string > constants; - TokenStruct< std::string > objects; - - Type() - : supertype( 0 ) {} - - Type( const std::string & s ) - : name( s ), supertype( 0 ) {} - - Type( const Type * t ) - : name( t->name ), supertype( 0 ), constants( t->constants ), objects( t->objects ) {} - - virtual ~Type() { - } - - virtual std::string getName() const { - return name; - } - - virtual void getSubTypesNames( std::vector & typesNames) const { - for (Type subtype : subtypes ) { - typesNames.push_back(subtype.name); - } - } - - void insertSubtype( Type * t ) { - subtypes.push_back( t ); - t->supertype = this; - } - - void copySubtypes( Type * t, const TokenStruct< Type * > & ts ) { - for ( unsigned i = 0; i < t->subtypes.size(); ++i ) - subtypes.push_back( ts.get( t->subtypes[i]->name ) ); - } - - void print( std::ostream & stream ) const { - stream << name; - if ( supertype ) stream << "[" << supertype->name << "]"; - stream << "\n"; - } - - virtual void PDDLPrint( std::ostream & s ) const { - s << "\t" << name; - if ( supertype ) s << " - " << supertype->name; - s << "\n"; - } - - std::pair< bool, int > parseConstant( const std::string & object ) { - int k = 0; - - int i = constants.index( object ); - if ( i < 0 ) k += constants.size(); - else return std::make_pair( true, -1 - i ); - - for ( unsigned i = 0; i < subtypes.size(); ++i ) { - std::pair< bool, int > p = subtypes[i]->parseConstant( object ); - if ( p.first ) return std::make_pair( true, - k + p.second ); - else k += p.second; - } - - return std::make_pair( false, k ); - } - - std::pair< bool, unsigned > parseObject( const std::string & object ) { - unsigned k = 0; - - int i = objects.index( object ); - if ( i < 0 ) k += objects.size(); - else return std::make_pair( true, i ); - - for ( unsigned i = 0; i < subtypes.size(); ++i ) { - std::pair< bool, unsigned > p = subtypes[i]->parseObject( object ); - if ( p.first ) return std::make_pair( true, k + p.second ); - else k += p.second; - } - - return std::make_pair( false, k ); - } - - std::pair< std::string, int > object( int index ) { - if ( index < 0 ) { - if ( -index <= (int)constants.size() ) return std::make_pair( constants[-1 - index], 0 ); - else index += constants.size(); - } - else { - if ( index < (int)objects.size() ) return std::make_pair( objects[index], 0 ); - else index -= objects.size(); - } - - for ( unsigned i = 0; i < subtypes.size(); ++i ) { - std::pair< std::string, int > p = subtypes[i]->object( index ); - if ( p.first.size() ) return p; - else index = p.second; - } - - return std::make_pair( "", index ); - } - - unsigned noObjects() { - unsigned total = objects.size() + constants.size(); - for ( unsigned i = 0; i < subtypes.size(); ++i ) - total += subtypes[i]->noObjects(); - return total; - } - - unsigned noConstants() { - unsigned total = constants.size(); - for ( unsigned i = 0; i < subtypes.size(); ++i ) { - total += subtypes[i]->noConstants(); - } - return total; - } - - virtual Type * copy() { - return new Type( this ); - } - + std::string name; + TypeVec subtypes; + Type * supertype; + + TokenStruct constants; + TokenStruct objects; + + Type() : supertype(0) {} + + Type(const std::string & s) : name(s), supertype(0) {} + + Type(const Type * t) : name(t->name), supertype(0), constants(t->constants), objects(t->objects) + { + } + + virtual ~Type() {} + + virtual std::string getName() const { return name; } + + virtual void getSubTypesNames(std::vector & typesNames) const + { + for (Type subtype : subtypes) { + typesNames.push_back(subtype.name); + } + } + + void insertSubtype(Type * t) + { + subtypes.push_back(t); + t->supertype = this; + } + + void copySubtypes(Type * t, const TokenStruct & ts) + { + for (unsigned i = 0; i < t->subtypes.size(); ++i) + subtypes.push_back(ts.get(t->subtypes[i]->name)); + } + + void print(std::ostream & stream) const + { + stream << name; + if (supertype) stream << "[" << supertype->name << "]"; + stream << "\n"; + } + + virtual void PDDLPrint(std::ostream & s) const + { + s << "\t" << name; + if (supertype) s << " - " << supertype->name; + s << "\n"; + } + + std::pair parseConstant(const std::string & object) + { + int k = 0; + + int i = constants.index(object); + if (i < 0) + k += constants.size(); + else + return std::make_pair(true, -1 - i); + + for (unsigned i = 0; i < subtypes.size(); ++i) { + std::pair p = subtypes[i]->parseConstant(object); + if (p.first) + return std::make_pair(true, -k + p.second); + else + k += p.second; + } + + return std::make_pair(false, k); + } + + std::pair parseObject(const std::string & object) + { + unsigned k = 0; + + int i = objects.index(object); + if (i < 0) + k += objects.size(); + else + return std::make_pair(true, i); + + for (unsigned i = 0; i < subtypes.size(); ++i) { + std::pair p = subtypes[i]->parseObject(object); + if (p.first) + return std::make_pair(true, k + p.second); + else + k += p.second; + } + + return std::make_pair(false, k); + } + + std::pair object(int index) + { + if (index < 0) { + if (-index <= (int)constants.size()) + return std::make_pair(constants[-1 - index], 0); + else + index += constants.size(); + } else { + if (index < (int)objects.size()) + return std::make_pair(objects[index], 0); + else + index -= objects.size(); + } + + for (unsigned i = 0; i < subtypes.size(); ++i) { + std::pair p = subtypes[i]->object(index); + if (p.first.size()) + return p; + else + index = p.second; + } + + return std::make_pair("", index); + } + + unsigned noObjects() + { + unsigned total = objects.size() + constants.size(); + for (unsigned i = 0; i < subtypes.size(); ++i) total += subtypes[i]->noObjects(); + return total; + } + + unsigned noConstants() + { + unsigned total = constants.size(); + for (unsigned i = 0; i < subtypes.size(); ++i) { + total += subtypes[i]->noConstants(); + } + return total; + } + + virtual Type * copy() { return new Type(this); } }; -inline std::ostream & operator<<( std::ostream & stream, const Type * t ) { - t->print( stream ); - return stream; +inline std::ostream & operator<<(std::ostream & stream, const Type * t) +{ + t->print(stream); + return stream; } -} } // namespaces +} // namespace pddl +} // namespace parser diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/TypeGround.h b/plansys2_pddl_parser/include/plansys2_pddl_parser/TypeGround.h index c750ab85..58e0a5b3 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/TypeGround.h +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/TypeGround.h @@ -3,34 +3,36 @@ #include "plansys2_msgs/msg/node.hpp" #include "plansys2_msgs/msg/tree.hpp" - #include "plansys2_pddl_parser/Ground.h" -namespace parser { namespace pddl { - -class TypeGround : public Ground { - +namespace parser +{ +namespace pddl +{ +class TypeGround : public Ground +{ public: + TypeGround() : Ground() {} - TypeGround() - : Ground() {} - - TypeGround( Lifted * l, const IntVec & p = IntVec() ) - : Ground( l, p ) {} - -// TypeGround( const TypeGround * tg ) -// : Ground( tg ) {} + TypeGround(Lifted * l, const IntVec & p = IntVec()) : Ground(l, p) {} - void PDDLPrint( std::ostream & s, unsigned indent, const TokenStruct< std::string > & ts, const Domain & d ) const override; + // TypeGround( const TypeGround * tg ) + // : Ground( tg ) {} - plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace = {} ) const override; + void PDDLPrint( + std::ostream & s, unsigned indent, const TokenStruct & ts, + const Domain & d) const override; - void insert( Domain & d, const StringVec & v ); + plansys2_msgs::msg::Node::SharedPtr getTree( + plansys2_msgs::msg::Tree & tree, const Domain & d, + const std::vector & replace = {}) const override; - void parse( Stringreader & f, TokenStruct< std::string > & ts, Domain & d ); + void insert(Domain & d, const StringVec & v); + void parse(Stringreader & f, TokenStruct & ts, Domain & d); }; -typedef std::vector< TypeGround * > TypeGroundVec; +typedef std::vector TypeGroundVec; -} } // namespaces +} // namespace pddl +} // namespace parser diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/Utils.h b/plansys2_pddl_parser/include/plansys2_pddl_parser/Utils.h index a90fd148..6ef8164b 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/Utils.h +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/Utils.h @@ -25,7 +25,6 @@ namespace parser { namespace pddl { - /// Removes newlines, duplicated spaces, tabs and spaces from parenthesis /** * \param[in] expr The expression to be reduced @@ -47,7 +46,7 @@ uint8_t getNodeType(const std::string & expr, uint8_t def = plansys2_msgs::msg:: * result(0) The expression type * result(1) The start position of the first occurrence */ -std::tuple < uint8_t, int > getExpr(const std::string & input); +std::tuple getExpr(const std::string & input); /// Returns expression type corresponding to the string input /** @@ -56,14 +55,15 @@ std::tuple < uint8_t, int > getExpr(const std::string & input); */ uint8_t getExprType(const std::string & input); -/// Returns function modifier type and start position of a function modifier in a string +/// Returns function modifier type and start position of a function modifier in +/// a string /** * \param[in] input The input string * \return result <- tuple(uint8_t, int) * result(0) The function modifier type * result(1) The start position of the first occurrence */ -std::tuple < uint8_t, int > getFunMod(const std::string & input); +std::tuple getFunMod(const std::string & input); /// Returns function modifier type corresponding to the string input /** @@ -80,7 +80,8 @@ std::string nameActionsToString(const std::shared_ptr action); -std::string toString(const plansys2_msgs::msg::Tree & tree, uint32_t node_id = 0, bool negate = false); +std::string toString( + const plansys2_msgs::msg::Tree & tree, uint32_t node_id = 0, bool negate = false); std::string toString(const plansys2_msgs::msg::Node & node); @@ -96,22 +97,29 @@ std::string toStringOr(const plansys2_msgs::msg::Tree & tree, uint32_t node_id, std::string toStringNot(const plansys2_msgs::msg::Tree & tree, uint32_t node_id, bool negate); -std::string toStringExpression(const plansys2_msgs::msg::Tree & tree, uint32_t node_id, bool negate); +std::string toStringExpression( + const plansys2_msgs::msg::Tree & tree, uint32_t node_id, bool negate); -std::string toStringFunctionModifier(const plansys2_msgs::msg::Tree & tree, uint32_t node_id, bool negate); +std::string toStringFunctionModifier( + const plansys2_msgs::msg::Tree & tree, uint32_t node_id, bool negate); /// This function creates a complete tree. /** - * This function recursivelly extracts the logic expressions and predicates from the expression. + * This function recursively extracts the logic expressions and predicates from + * the expression. * * \param[in] tree The tree object to be created * \param[in] expr A string containing predicates and logic operators * \param[in] construct A string containing the associated PDDL constructs * \return A smart pointer to the node created -*/ -plansys2_msgs::msg::Node::SharedPtr fromString(plansys2_msgs::msg::Tree & tree, const std::string & expr, bool negate = false, uint8_t parent = plansys2_msgs::msg::Node::UNKNOWN); + */ +plansys2_msgs::msg::Node::SharedPtr fromString( + plansys2_msgs::msg::Tree & tree, const std::string & expr, bool negate = false, + uint8_t parent = plansys2_msgs::msg::Node::UNKNOWN); -plansys2_msgs::msg::Tree fromString(const std::string & expr, bool negate = false, uint8_t parent = plansys2_msgs::msg::Node::UNKNOWN); +plansys2_msgs::msg::Tree fromString( + const std::string & expr, bool negate = false, + uint8_t parent = plansys2_msgs::msg::Node::UNKNOWN); plansys2_msgs::msg::Node fromStringPredicate(const std::string & predicate); @@ -121,25 +129,36 @@ plansys2_msgs::msg::Param fromStringParam(const std::string & name, const std::s plansys2_msgs::msg::Tree fromPredicates(const std::vector & preds); -plansys2_msgs::msg::Tree::SharedPtr fromSubtree(const plansys2_msgs::msg::Tree & subtree, uint8_t node_type); +plansys2_msgs::msg::Tree::SharedPtr fromSubtree( + const plansys2_msgs::msg::Tree & subtree, uint8_t node_type); -plansys2_msgs::msg::Tree::SharedPtr fromSubtrees(const std::vector & subtrees, uint8_t node_type); +plansys2_msgs::msg::Tree::SharedPtr fromSubtrees( + const std::vector & subtrees, uint8_t node_type); std::vector getSubtreeIds(const plansys2_msgs::msg::Tree & tree); std::vector getSubtrees(const plansys2_msgs::msg::Tree & tree); -void getSubtreeChildren(plansys2_msgs::msg::Tree & subtree, const plansys2_msgs::msg::Tree & tree, uint32_t tree_parent, uint32_t subtree_parent); +void getSubtreeChildren( + plansys2_msgs::msg::Tree & subtree, const plansys2_msgs::msg::Tree & tree, uint32_t tree_parent, + uint32_t subtree_parent); -void getPredicates(std::vector & predicates, const plansys2_msgs::msg::Tree & tree, uint32_t node_id = 0, bool negate = false); +void getPredicates( + std::vector & predicates, const plansys2_msgs::msg::Tree & tree, + uint32_t node_id = 0, bool negate = false); -void getFunctions(std::vector & functions, const plansys2_msgs::msg::Tree & tree, uint32_t node_id = 0, bool negate = false); +void getFunctions( + std::vector & functions, const plansys2_msgs::msg::Tree & tree, + uint32_t node_id = 0, bool negate = false); -bool checkTreeEquality(const plansys2_msgs::msg::Tree & first, const plansys2_msgs::msg::Tree & second); +bool checkTreeEquality( + const plansys2_msgs::msg::Tree & first, const plansys2_msgs::msg::Tree & second); -bool checkNodeEquality(const plansys2_msgs::msg::Node & first, const plansys2_msgs::msg::Node & second); +bool checkNodeEquality( + const plansys2_msgs::msg::Node & first, const plansys2_msgs::msg::Node & second); -bool checkParamEquality(const plansys2_msgs::msg::Param & first, const plansys2_msgs::msg::Param & second); +bool checkParamEquality( + const plansys2_msgs::msg::Param & first, const plansys2_msgs::msg::Param & second); bool empty(const plansys2_msgs::msg::Tree & tree); diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/When.h b/plansys2_pddl_parser/include/plansys2_pddl_parser/When.h index d01658f5..a70c432c 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/When.h +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/When.h @@ -3,52 +3,56 @@ #include "plansys2_msgs/msg/node.hpp" #include "plansys2_msgs/msg/tree.hpp" - #include "plansys2_pddl_parser/Condition.h" -namespace parser { namespace pddl { - -class When : public Condition { - +namespace parser +{ +namespace pddl +{ +class When : public Condition +{ public: - - Condition *pars, *cond; - - When() - : pars( 0 ), cond( 0 ) {} - - When( const When * w, Domain & d ) - : pars( 0 ), cond( 0 ) { - if ( w->pars ) pars = w->pars->copy( d ); - if ( w->cond ) cond = w->cond->copy( d ); - } - - ~When() { - if ( pars ) delete pars; - if ( cond ) delete cond; - } - - void print( std::ostream & s ) const { - s << "when:\n"; - if ( pars ) pars->print( s ); - if ( cond ) cond->print( s ); - } - - void PDDLPrint( std::ostream & s, unsigned indent, const TokenStruct< std::string > & ts, const Domain & d ) const override; - - plansys2_msgs::msg::Node::SharedPtr getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace = {} ) const override; - - void parse( Stringreader & f, TokenStruct< std::string > & ts, Domain & d ); - - void addParams( int m, unsigned n ) { - pars->addParams( m, n ); - cond->addParams( m, n ); - } - - Condition * copy( Domain & d ) { - return new When( this, d ); - } - + Condition *pars, *cond; + + When() : pars(0), cond(0) {} + + When(const When * w, Domain & d) : pars(0), cond(0) + { + if (w->pars) pars = w->pars->copy(d); + if (w->cond) cond = w->cond->copy(d); + } + + ~When() + { + if (pars) delete pars; + if (cond) delete cond; + } + + void print(std::ostream & s) const + { + s << "when:\n"; + if (pars) pars->print(s); + if (cond) cond->print(s); + } + + void PDDLPrint( + std::ostream & s, unsigned indent, const TokenStruct & ts, + const Domain & d) const override; + + plansys2_msgs::msg::Node::SharedPtr getTree( + plansys2_msgs::msg::Tree & tree, const Domain & d, + const std::vector & replace = {}) const override; + + void parse(Stringreader & f, TokenStruct & ts, Domain & d); + + void addParams(int m, unsigned n) + { + pars->addParams(m, n); + cond->addParams(m, n); + } + + Condition * copy(Domain & d) { return new When(this, d); } }; -} } // namespaces +} // namespace pddl +} // namespace parser diff --git a/plansys2_pddl_parser/package.xml b/plansys2_pddl_parser/package.xml index 6cab24c0..586f036a 100644 --- a/plansys2_pddl_parser/package.xml +++ b/plansys2_pddl_parser/package.xml @@ -3,9 +3,9 @@ plansys2_pddl_parser 2.0.8 - + This package contains a library for parsing PDDL domains and problems. - + This package derives from the work of Anders Jonsson, contained in https://github.com/wisdompoet/universal-pddl-parser.git with many modifications by Francisco Martin: * ROS2 packaging @@ -16,7 +16,7 @@ Francisco Martin Rico - + Apache License, Version 2.0 Anders Jonsson diff --git a/plansys2_pddl_parser/src/parser.cpp b/plansys2_pddl_parser/src/parser.cpp index b04e5a74..8c83ba80 100644 --- a/plansys2_pddl_parser/src/parser.cpp +++ b/plansys2_pddl_parser/src/parser.cpp @@ -1,32 +1,30 @@ -#include -#include #include +#include #include - +#include #include "plansys2_pddl_parser/Instance.h" using namespace parser::pddl; -int main( int argc, char *argv[] ) { - if ( argc < 3 ) { - std::cout << "Usage: parser \n"; - exit( 1 ); - } - std::ifstream domain_ifs(argv[1]); - std::string domain_str(( - std::istreambuf_iterator(domain_ifs)), - std::istreambuf_iterator()); +int main(int argc, char * argv[]) +{ + if (argc < 3) { + std::cout << "Usage: parser \n"; + exit(1); + } + std::ifstream domain_ifs(argv[1]); + std::string domain_str( + (std::istreambuf_iterator(domain_ifs)), std::istreambuf_iterator()); - std::ifstream instance_ifs(argv[2]); - std::string instance_str(( - std::istreambuf_iterator(instance_ifs)), - std::istreambuf_iterator()); + std::ifstream instance_ifs(argv[2]); + std::string instance_str( + (std::istreambuf_iterator(instance_ifs)), std::istreambuf_iterator()); - // Read multiagent domain and instance - Domain domain( domain_str ); - Instance instance( domain, instance_str ); + // Read multiagent domain and instance + Domain domain(domain_str); + Instance instance(domain, instance_str); - std::cout << domain << std::endl; - std::cout << instance << std::endl; -} \ No newline at end of file + std::cout << domain << std::endl; + std::cout << instance << std::endl; +} diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/Action.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/Action.cpp index f51b9a83..b38c3d0e 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/Action.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/Action.cpp @@ -1,139 +1,144 @@ #include "plansys2_pddl_parser/Domain.h" -namespace parser { namespace pddl { - -void Action::PDDLPrint( std::ostream & s, unsigned indent, const TokenStruct< std::string > & ts, const Domain & d ) const { - s << "( :action " << name << "\n"; - - s << " :parameters "; - - TokenStruct< std::string > astruct; - - printParams( 0, s, astruct, d ); - - s << " :precondition\n"; - if ( pre ) pre->PDDLPrint( s, 1, astruct, d ); - else s << "\t()"; - s << "\n"; - - s << " :effect\n"; - if ( eff ) eff->PDDLPrint( s, 1, astruct, d ); - else s << "\t()"; - s << "\n"; - - s << ")\n"; +namespace parser +{ +namespace pddl +{ +void Action::PDDLPrint( + std::ostream & s, unsigned indent, const TokenStruct & ts, const Domain & d) const +{ + s << "( :action " << name << "\n"; + + s << " :parameters "; + + TokenStruct astruct; + + printParams(0, s, astruct, d); + + s << " :precondition\n"; + if (pre) + pre->PDDLPrint(s, 1, astruct, d); + else + s << "\t()"; + s << "\n"; + + s << " :effect\n"; + if (eff) + eff->PDDLPrint(s, 1, astruct, d); + else + s << "\t()"; + s << "\n"; + + s << ")\n"; } -plansys2_msgs::msg::Node::SharedPtr Action::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace ) const { - plansys2_msgs::msg::Node::SharedPtr node = std::make_shared(); - node->node_type = plansys2_msgs::msg::Node::ACTION; - node->node_id = tree.nodes.size(); - tree.nodes.push_back(*node); - - if (pre) { - plansys2_msgs::msg::Node::SharedPtr child = pre->getTree(tree, d, replace); - tree.nodes[node->node_id].children.push_back(child->node_id); - } - - if (eff) { - plansys2_msgs::msg::Node::SharedPtr child = eff->getTree(tree, d, replace); - tree.nodes[node->node_id].children.push_back(child->node_id); - } - - return node; +plansys2_msgs::msg::Node::SharedPtr Action::getTree( + plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace) const +{ + plansys2_msgs::msg::Node::SharedPtr node = std::make_shared(); + node->node_type = plansys2_msgs::msg::Node::ACTION; + node->node_id = tree.nodes.size(); + tree.nodes.push_back(*node); + + if (pre) { + plansys2_msgs::msg::Node::SharedPtr child = pre->getTree(tree, d, replace); + tree.nodes[node->node_id].children.push_back(child->node_id); + } + + if (eff) { + plansys2_msgs::msg::Node::SharedPtr child = eff->getTree(tree, d, replace); + tree.nodes[node->node_id].children.push_back(child->node_id); + } + + return node; } -void Action::parseConditions( Stringreader & f, TokenStruct< std::string > & ts, Domain & d ) { - f.next(); - f.assert_token( ":" ); - std::string s = f.getToken(); - if ( s == "precondition" ) { - f.next(); - f.assert_token( "(" ); - if ( f.getChar() != ')' ) { - pre = d.createCondition( f ); - pre->parse( f, ts, d ); - } - else ++f.c; - - f.next(); - f.assert_token( ":" ); - s = f.getToken(); - } - if ( s != "effect" ) f.tokenExit( s ); - - f.next(); - f.assert_token( "(" ); - if ( f.getChar() != ')' ) { - eff = d.createCondition( f ); - eff->parse( f, ts, d ); - } - else ++f.c; - f.next(); - f.assert_token( ")" ); +void Action::parseConditions(Stringreader & f, TokenStruct & ts, Domain & d) +{ + f.next(); + f.assert_token(":"); + std::string s = f.getToken(); + if (s == "precondition") { + f.next(); + f.assert_token("("); + if (f.getChar() != ')') { + pre = d.createCondition(f); + pre->parse(f, ts, d); + } else + ++f.c; + + f.next(); + f.assert_token(":"); + s = f.getToken(); + } + if (s != "effect") f.tokenExit(s); + + f.next(); + f.assert_token("("); + if (f.getChar() != ')') { + eff = d.createCondition(f); + eff->parse(f, ts, d); + } else + ++f.c; + f.next(); + f.assert_token(")"); } -void Action::parse( Stringreader & f, TokenStruct< std::string > & ts, Domain & d ) { - f.next(); - f.assert_token( ":parameters" ); - f.assert_token( "(" ); +void Action::parse(Stringreader & f, TokenStruct & ts, Domain & d) +{ + f.next(); + f.assert_token(":parameters"); + f.assert_token("("); - TokenStruct< std::string > astruct = f.parseTypedList( true, d.types ); - params = d.convertTypes( astruct.types ); + TokenStruct astruct = f.parseTypedList(true, d.types); + params = d.convertTypes(astruct.types); - parseConditions( f, astruct, d ); + parseConditions(f, astruct, d); } -CondVec Action::precons() { - return getSubconditionsFromCondition( pre ); -} +CondVec Action::precons() { return getSubconditionsFromCondition(pre); } -CondVec Action::effects() { - return getSubconditionsFromCondition( eff ); -} +CondVec Action::effects() { return getSubconditionsFromCondition(eff); } -GroundVec Action::addEffects() { - return getGroundsFromCondition( eff, false ); -} +GroundVec Action::addEffects() { return getGroundsFromCondition(eff, false); } -GroundVec Action::deleteEffects() { - return getGroundsFromCondition( eff, true ); -} +GroundVec Action::deleteEffects() { return getGroundsFromCondition(eff, true); } -CondVec Action::getSubconditionsFromCondition( Condition * c ) { - And * a = dynamic_cast< And * >( c ); - if ( a ) return a->conds; +CondVec Action::getSubconditionsFromCondition(Condition * c) +{ + And * a = dynamic_cast(c); + if (a) return a->conds; - CondVec subconds; - if ( c ) subconds.push_back( c ); - return subconds; + CondVec subconds; + if (c) subconds.push_back(c); + return subconds; } -GroundVec Action::getGroundsFromCondition( Condition * c, bool neg ) { - GroundVec grounds; - And * a = dynamic_cast< And * >( c ); - for ( unsigned i = 0; a && i < a->conds.size(); ++i ) { - if ( neg ) { - Not * n = dynamic_cast< Not * >( a->conds[i] ); - if ( n ) grounds.push_back( n->cond ); - } - else { - Ground * g = dynamic_cast< Ground * >( a->conds[i] ); - if ( g ) grounds.push_back( g ); - } - } - - if ( neg ) { - Not * n = dynamic_cast< Not * >( c ); - if ( n ) grounds.push_back( n->cond ); - } - else { - Ground * g = dynamic_cast< Ground * >( c ); - if ( g ) grounds.push_back( g ); - } - - return grounds; +GroundVec Action::getGroundsFromCondition(Condition * c, bool neg) +{ + GroundVec grounds; + And * a = dynamic_cast(c); + for (unsigned i = 0; a && i < a->conds.size(); ++i) { + if (neg) { + Not * n = dynamic_cast(a->conds[i]); + if (n) grounds.push_back(n->cond); + } else { + Ground * g = dynamic_cast(a->conds[i]); + if (g) grounds.push_back(g); + } + } + + if (neg) { + Not * n = dynamic_cast(c); + if (n) grounds.push_back(n->cond); + } else { + Ground * g = dynamic_cast(c); + if (g) grounds.push_back(g); + } + + return grounds; } -} } // namespaces +} // namespace pddl +} // namespace parser diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/And.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/And.cpp index f75ac4ac..c8bf7b14 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/And.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/And.cpp @@ -1,41 +1,49 @@ #include "plansys2_pddl_parser/Domain.h" -namespace parser { namespace pddl { - -void And::PDDLPrint( std::ostream & s, unsigned indent, const TokenStruct< std::string > & ts, const Domain & d ) const { - tabindent( s, indent ); - s << "( and\n"; - for ( unsigned i = 0; i < conds.size(); ++i ) { - conds[i]->PDDLPrint( s, indent + 1, ts, d ); - s << "\n"; - } - tabindent( s, indent ); - s << ")"; +namespace parser +{ +namespace pddl +{ +void And::PDDLPrint( + std::ostream & s, unsigned indent, const TokenStruct & ts, const Domain & d) const +{ + tabindent(s, indent); + s << "( and\n"; + for (unsigned i = 0; i < conds.size(); ++i) { + conds[i]->PDDLPrint(s, indent + 1, ts, d); + s << "\n"; + } + tabindent(s, indent); + s << ")"; } -plansys2_msgs::msg::Node::SharedPtr And::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace ) const { - plansys2_msgs::msg::Node::SharedPtr node = std::make_shared(); - node->node_type = plansys2_msgs::msg::Node::AND; - node->node_id = tree.nodes.size(); - tree.nodes.push_back(*node); +plansys2_msgs::msg::Node::SharedPtr And::getTree( + plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace) const +{ + plansys2_msgs::msg::Node::SharedPtr node = std::make_shared(); + node->node_type = plansys2_msgs::msg::Node::AND; + node->node_id = tree.nodes.size(); + tree.nodes.push_back(*node); - for ( unsigned i = 0; i < conds.size(); ++i) { - plansys2_msgs::msg::Node::SharedPtr child = conds[i]->getTree(tree, d, replace); - tree.nodes[node->node_id].children.push_back(child->node_id); - } + for (unsigned i = 0; i < conds.size(); ++i) { + plansys2_msgs::msg::Node::SharedPtr child = conds[i]->getTree(tree, d, replace); + tree.nodes[node->node_id].children.push_back(child->node_id); + } - return node; + return node; } -void And::parse( Stringreader & f, TokenStruct< std::string > & ts, Domain & d ) { - for ( f.next(); f.getChar() != ')'; f.next() ) { - f.assert_token( "(" ); - Condition * condition = d.createCondition( f ); - condition->parse( f, ts, d ); - conds.push_back( condition ); - } - ++f.c; +void And::parse(Stringreader & f, TokenStruct & ts, Domain & d) +{ + for (f.next(); f.getChar() != ')'; f.next()) { + f.assert_token("("); + Condition * condition = d.createCondition(f); + condition->parse(f, ts, d); + conds.push_back(condition); + } + ++f.c; } -} } // namespaces +} // namespace pddl +} // namespace parser diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/Derived.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/Derived.cpp index 664ca2a6..b1937847 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/Derived.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/Derived.cpp @@ -1,50 +1,58 @@ #include "plansys2_pddl_parser/Domain.h" -namespace parser { namespace pddl { - -Derived::Derived( const Derived * z, Domain & d ) - : Lifted( z ), cond( 0 ), lifted( d.preds.get( z->name ) ) { - if ( z->cond ) cond = z->cond->copy( d ); +namespace parser +{ +namespace pddl +{ +Derived::Derived(const Derived * z, Domain & d) : Lifted(z), cond(0), lifted(d.preds.get(z->name)) +{ + if (z->cond) cond = z->cond->copy(d); } -void Derived::PDDLPrint( std::ostream & s, unsigned indent, const TokenStruct< std::string > & ts, const Domain & d ) const { - s << "( :derived ( " << name; +void Derived::PDDLPrint( + std::ostream & s, unsigned indent, const TokenStruct & ts, const Domain & d) const +{ + s << "( :derived ( " << name; - TokenStruct< std::string > dstruct( ts ); + TokenStruct dstruct(ts); - for ( unsigned i = 0; i < params.size(); ++i ) { - std::stringstream ss; - ss << "?" << d.types[params[i]]->getName() << dstruct.size(); - dstruct.insert( ss.str() ); - s << " " << ss.str(); - if ( d.typed ) s << " - " << d.types[params[i]]->name; - } - s << " )\n"; + for (unsigned i = 0; i < params.size(); ++i) { + std::stringstream ss; + ss << "?" << d.types[params[i]]->getName() << dstruct.size(); + dstruct.insert(ss.str()); + s << " " << ss.str(); + if (d.typed) s << " - " << d.types[params[i]]->name; + } + s << " )\n"; - if ( cond ) cond->PDDLPrint( s, 1, dstruct, d ); + if (cond) cond->PDDLPrint(s, 1, dstruct, d); - s << "\n)\n"; + s << "\n)\n"; } -plansys2_msgs::msg::Node::SharedPtr Derived::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace ) const { - throw UnsupportedConstruct("Derived"); +plansys2_msgs::msg::Node::SharedPtr Derived::getTree( + plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace) const +{ + throw UnsupportedConstruct("Derived"); } -void Derived::parse( Stringreader & f, TokenStruct< std::string > & ts, Domain & d ) { - f.next(); - f.assert_token( "(" ); - name = f.getToken( d.preds ); - TokenStruct< std::string > dstruct = f.parseTypedList( true, d.types ); - params = d.convertTypes( dstruct.types ); - - f.next(); - f.assert_token( "(" ); - cond = d.createCondition( f ); - cond->parse( f, dstruct, d ); - - f.next(); - f.assert_token( ")" ); +void Derived::parse(Stringreader & f, TokenStruct & ts, Domain & d) +{ + f.next(); + f.assert_token("("); + name = f.getToken(d.preds); + TokenStruct dstruct = f.parseTypedList(true, d.types); + params = d.convertTypes(dstruct.types); + + f.next(); + f.assert_token("("); + cond = d.createCondition(f); + cond->parse(f, dstruct, d); + + f.next(); + f.assert_token(")"); } -} } // namespaces +} // namespace pddl +} // namespace parser diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/Exists.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/Exists.cpp index f71f5295..d74db88b 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/Exists.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/Exists.cpp @@ -1,51 +1,60 @@ #include "plansys2_pddl_parser/Domain.h" -namespace parser { namespace pddl { - -void Exists::PDDLPrint( std::ostream & s, unsigned indent, const TokenStruct< std::string > & ts, const Domain & d ) const { - tabindent( s, indent ); - s << "( exists\n"; - - TokenStruct< std::string > fstruct( ts ); - - tabindent( s, indent + 1 ); - printParams( 0, s, fstruct, d ); - - if ( cond ) cond->PDDLPrint( s, indent + 1, fstruct, d ); - else { - tabindent( s, indent + 1 ); - s << "()"; - } - s << "\n"; - tabindent( s, indent ); - s << ")"; +namespace parser +{ +namespace pddl +{ +void Exists::PDDLPrint( + std::ostream & s, unsigned indent, const TokenStruct & ts, const Domain & d) const +{ + tabindent(s, indent); + s << "( exists\n"; + + TokenStruct fstruct(ts); + + tabindent(s, indent + 1); + printParams(0, s, fstruct, d); + + if (cond) + cond->PDDLPrint(s, indent + 1, fstruct, d); + else { + tabindent(s, indent + 1); + s << "()"; + } + s << "\n"; + tabindent(s, indent); + s << ")"; } -plansys2_msgs::msg::Node::SharedPtr Exists::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace ) const { - throw UnsupportedConstruct("Exists"); +plansys2_msgs::msg::Node::SharedPtr Exists::getTree( + plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace) const +{ + throw UnsupportedConstruct("Exists"); } -void Exists::parse( Stringreader & f, TokenStruct< std::string > & ts, Domain & d ) { - f.next(); - f.assert_token( "(" ); +void Exists::parse(Stringreader & f, TokenStruct & ts, Domain & d) +{ + f.next(); + f.assert_token("("); - TokenStruct< std::string > es = f.parseTypedList( true, d.types ); - params = d.convertTypes( es.types ); + TokenStruct es = f.parseTypedList(true, d.types); + params = d.convertTypes(es.types); - TokenStruct< std::string > estruct( ts ); - estruct.append( es ); + TokenStruct estruct(ts); + estruct.append(es); - f.next(); - f.assert_token( "(" ); - if ( f.getChar() != ')' ) { - cond = d.createCondition( f ); - cond->parse( f, estruct, d ); - } - else ++f.c; + f.next(); + f.assert_token("("); + if (f.getChar() != ')') { + cond = d.createCondition(f); + cond->parse(f, estruct, d); + } else + ++f.c; - f.next(); - f.assert_token( ")" ); + f.next(); + f.assert_token(")"); } -} } // namespaces +} // namespace pddl +} // namespace parser diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/Expression.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/Expression.cpp index d4e13e03..ebe2ae9b 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/Expression.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/Expression.cpp @@ -1,160 +1,175 @@ #include "plansys2_pddl_parser/Instance.h" -namespace parser { namespace pddl { +namespace parser +{ +namespace pddl +{ +void FunctionExpression::PDDLPrint( + std::ostream & s, unsigned indent, const TokenStruct & ts, const Domain & d) const +{ + ParamCond * c = d.funcs[d.funcs.index(fun->name)]; -void FunctionExpression::PDDLPrint( std::ostream & s, unsigned indent, const TokenStruct< std::string > & ts, const Domain & d ) const { - ParamCond * c = d.funcs[d.funcs.index( fun->name )]; - - s << "( " << fun->name; - unsigned int psize = c->params.size(); - for ( unsigned i = 0; i < psize; ++i ) { - if ( ts.size() && fun->params[i] >= 0 && (unsigned)fun->params[i] < ts.size() ) s << " " << ts[fun->params[i]]; - else if (fun->params[i] >= 0 && (unsigned)fun->params[i] >= ts.size()) s << " ?" << fun->params[i]; - else { - // It is a constant parameter for the function - s << " "; constants[i]->PDDLPrint(s, indent, ts, d); + s << "( " << fun->name; + unsigned int psize = c->params.size(); + for (unsigned i = 0; i < psize; ++i) { + if (ts.size() && fun->params[i] >= 0 && (unsigned)fun->params[i] < ts.size()) + s << " " << ts[fun->params[i]]; + else if (fun->params[i] >= 0 && (unsigned)fun->params[i] >= ts.size()) + s << " ?" << fun->params[i]; + else { + // It is a constant parameter for the function + s << " "; + constants[i]->PDDLPrint(s, indent, ts, d); + } } - } - s << " )"; + s << " )"; } -plansys2_msgs::msg::Node::SharedPtr FunctionExpression::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace ) const { - plansys2_msgs::msg::Node::SharedPtr node = std::make_shared(); - node->node_type = plansys2_msgs::msg::Node::FUNCTION; - node->node_id = tree.nodes.size(); - node->name = fun->name; - for ( unsigned i = 0; i < fun->params.size(); ++i ) { - plansys2_msgs::msg::Param param; - if (i < replace.size()) { - param.name = replace[fun->params[i]]; - } else { - param.name = "?" + std::to_string(fun->params[i]); - } - param.type = d.types[fun->params[i]]->name; - node->parameters.push_back(param); +plansys2_msgs::msg::Node::SharedPtr FunctionExpression::getTree( + plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace) const +{ + plansys2_msgs::msg::Node::SharedPtr node = std::make_shared(); + node->node_type = plansys2_msgs::msg::Node::FUNCTION; + node->node_id = tree.nodes.size(); + node->name = fun->name; + for (unsigned i = 0; i < fun->params.size(); ++i) { + plansys2_msgs::msg::Param param; + if (i < replace.size()) { + param.name = replace[fun->params[i]]; + } else { + param.name = "?" + std::to_string(fun->params[i]); } - tree.nodes.push_back(*node); - return node; + param.type = d.types[fun->params[i]]->name; + node->parameters.push_back(param); + } + tree.nodes.push_back(*node); + return node; } -double FunctionExpression::evaluate( Instance & ins, const StringVec & par ) { - ParamCond * c = ins.d.funcs[ins.d.funcs.index( fun->name )]; +double FunctionExpression::evaluate(Instance & ins, const StringVec & par) +{ + ParamCond * c = ins.d.funcs[ins.d.funcs.index(fun->name)]; - IntVec v( c->params.size() ); - for ( unsigned i = 0; i < v.size(); ++i ) { - std::pair< bool, unsigned > p = ins.d.types[c->params[i]]->parseObject( par[fun->params[i]] ); - if ( p.first ) v[i] = p.second; - else { - std::pair< bool, int > q = ins.d.types[c->params[i]]->parseConstant( par[fun->params[i]] ); - if ( q.first ) v[i] = q.second; - else return 1; - } - } + IntVec v(c->params.size()); + for (unsigned i = 0; i < v.size(); ++i) { + std::pair p = ins.d.types[c->params[i]]->parseObject(par[fun->params[i]]); + if (p.first) + v[i] = p.second; + else { + std::pair q = ins.d.types[c->params[i]]->parseConstant(par[fun->params[i]]); + if (q.first) + v[i] = q.second; + else + return 1; + } + } - for ( unsigned i = 0; i < ins.init.size(); ++i ) - if ( ins.init[i]->name == c->name && ins.init[i]->params == v ) - return ((GroundFunc *)ins.init[i])->value; - return 1; + for (unsigned i = 0; i < ins.init.size(); ++i) + if (ins.init[i]->name == c->name && ins.init[i]->params == v) + return ((GroundFunc *)ins.init[i])->value; + return 1; } +void ConstExpression::PDDLPrint( + std::ostream & s, unsigned indent, const TokenStruct & ts, const Domain & d) const +{ + s << d.types[tid]->constants[constant]; +} -void ConstExpression::PDDLPrint( std::ostream & s, unsigned indent, const TokenStruct< std::string > & ts, const Domain & d ) const { - s << d.types[tid]->constants[constant]; - } - -Expression * createExpression( Stringreader & f, TokenStruct< std::string > & ts, Domain & d ) { - f.next(); +Expression * createExpression(Stringreader & f, TokenStruct & ts, Domain & d) +{ + f.next(); - if ( f.getChar() == '(' ) { - ++f.c; - f.next(); - std::string s = f.getToken(); - if ( s == "+" || s == "-" || s == "*" || s == "/" ) { - // It is a composite expression - CompositeExpression * ce = new CompositeExpression( s ); - ce->parse( f, ts, d ); - return ce; - } else { - // It is a function expression - f.c -= s.size(); - Function * fun = d.funcs.get( f.getToken( d.funcs ) ); - ParamCond * p = new Lifted( fun ); - std::vector c(fun->params.size()); - std::fill(c.begin(), c.end(), nullptr); - for ( unsigned i = 0; i < fun->params.size(); ++i ) { + if (f.getChar() == '(') { + ++f.c; f.next(); std::string s = f.getToken(); - if (d.isConstant(s)) { - p->params[i] = -1; - IntPair ct = d.constantTypeIdConstId(s); - c[i] = new ConstExpression(ct.first, ct.second); - } else if (ts.index(s) >= 0) { - p->params[i] = ts.index( s ); + if (s == "+" || s == "-" || s == "*" || s == "/") { + // It is a composite expression + CompositeExpression * ce = new CompositeExpression(s); + ce->parse(f, ts, d); + return ce; } else { - f.tokenExit(s); + // It is a function expression + f.c -= s.size(); + Function * fun = d.funcs.get(f.getToken(d.funcs)); + ParamCond * p = new Lifted(fun); + std::vector c(fun->params.size()); + std::fill(c.begin(), c.end(), nullptr); + for (unsigned i = 0; i < fun->params.size(); ++i) { + f.next(); + std::string s = f.getToken(); + if (d.isConstant(s)) { + p->params[i] = -1; + IntPair ct = d.constantTypeIdConstId(s); + c[i] = new ConstExpression(ct.first, ct.second); + } else if (ts.index(s) >= 0) { + p->params[i] = ts.index(s); + } else { + f.tokenExit(s); + } + } + f.next(); + f.assert_token(")"); + return new FunctionExpression(p, c); } - } - f.next(); - f.assert_token( ")" ); - return new FunctionExpression( p, c ); - } - } else if ( f.getChar() == '?' ) { - // It is a parameter variable - std::string s = f.getToken(); + } else if (f.getChar() == '?') { + // It is a parameter variable + std::string s = f.getToken(); - int k = ts.index( s ); - if ( k >= 0) { - // It is a parameter variable - return new ParamExpression( k ); - } else if (s == "?duration") { - // It is a ?duration expression - return new DurationExpression(); - } else { - // Neither a known parameter variable nor ?duration - f.tokenExit(s); - return nullptr; - } - } else { - std::string s = f.getToken(); + int k = ts.index(s); + if (k >= 0) { + // It is a parameter variable + return new ParamExpression(k); + } else if (s == "?duration") { + // It is a ?duration expression + return new DurationExpression(); + } else { + // Neither a known parameter variable nor ?duration + f.tokenExit(s); + return nullptr; + } + } else { + std::string s = f.getToken(); - if ( d.isConstant(s) ) { - // It is a constant! - IntPair ct = d.constantTypeIdConstId(s); - return new ConstExpression(ct.first, ct.second); - } else if (s == "#t") { - // This construct is used in PDDL+ to model continuous evolution - // of some function within processes - std::ostringstream os; - os << "\"" << s << "\" is supported only in PDDL+," << - "and not supported by Plansys2" << std::endl; - f.printLine(); - std::cout << os.str(); - throw new std::runtime_error(os.str()); - return nullptr; - } else { - // It is expected to be a number! - double d; - try { - d = std::stod(s); - } catch (const std::invalid_argument &) { - std::ostringstream os; - os << "Expected a number, found \"" << s << "\"" << std::endl; - f.printLine(); - std::cout << os.str(); - throw new std::runtime_error(os.str()); - } catch (const std::out_of_range &) { - std::ostringstream os; - os << "Expected a number, found \"" << s << "\"" << std::endl << - "and it goes out of range" << std::endl; - f.printLine(); - std::cout << os.str(); - throw new std::runtime_error(os.str()); - } - return new ValueExpression( d ); - } - } + if (d.isConstant(s)) { + // It is a constant! + IntPair ct = d.constantTypeIdConstId(s); + return new ConstExpression(ct.first, ct.second); + } else if (s == "#t") { + // This construct is used in PDDL+ to model continuous evolution + // of some function within processes + std::ostringstream os; + os << "\"" << s << "\" is supported only in PDDL+," + << "and not supported by Plansys2" << std::endl; + f.printLine(); + std::cout << os.str(); + throw new std::runtime_error(os.str()); + return nullptr; + } else { + // It is expected to be a number! + double d; + try { + d = std::stod(s); + } catch (const std::invalid_argument &) { + std::ostringstream os; + os << "Expected a number, found \"" << s << "\"" << std::endl; + f.printLine(); + std::cout << os.str(); + throw new std::runtime_error(os.str()); + } catch (const std::out_of_range &) { + std::ostringstream os; + os << "Expected a number, found \"" << s << "\"" << std::endl + << "and it goes out of range" << std::endl; + f.printLine(); + std::cout << os.str(); + throw new std::runtime_error(os.str()); + } + return new ValueExpression(d); + } + } } -} -} +} // namespace pddl +} // namespace parser diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/Forall.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/Forall.cpp index 7c034704..f4b248da 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/Forall.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/Forall.cpp @@ -1,52 +1,61 @@ #include "plansys2_pddl_parser/Domain.h" -namespace parser { namespace pddl { - -void Forall::PDDLPrint( std::ostream & s, unsigned indent, const TokenStruct< std::string > & ts, const Domain & d ) const { - tabindent( s, indent ); - s << "( forall\n"; - - TokenStruct< std::string > fstruct( ts ); - - tabindent( s, indent + 1 ); - printParams( 0, s, fstruct, d ); - - if ( cond ) cond->PDDLPrint( s, indent + 1, fstruct, d ); - else { - tabindent( s, indent + 1 ); - s << "()"; - } - - s << "\n"; - tabindent( s, indent ); - s << ")"; +namespace parser +{ +namespace pddl +{ +void Forall::PDDLPrint( + std::ostream & s, unsigned indent, const TokenStruct & ts, const Domain & d) const +{ + tabindent(s, indent); + s << "( forall\n"; + + TokenStruct fstruct(ts); + + tabindent(s, indent + 1); + printParams(0, s, fstruct, d); + + if (cond) + cond->PDDLPrint(s, indent + 1, fstruct, d); + else { + tabindent(s, indent + 1); + s << "()"; + } + + s << "\n"; + tabindent(s, indent); + s << ")"; } -plansys2_msgs::msg::Node::SharedPtr Forall::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace ) const { - throw UnsupportedConstruct("Forall"); +plansys2_msgs::msg::Node::SharedPtr Forall::getTree( + plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace) const +{ + throw UnsupportedConstruct("Forall"); } -void Forall::parse( Stringreader & f, TokenStruct< std::string > & ts, Domain & d ) { - f.next(); - f.assert_token( "(" ); +void Forall::parse(Stringreader & f, TokenStruct & ts, Domain & d) +{ + f.next(); + f.assert_token("("); - TokenStruct< std::string > fs = f.parseTypedList( true, d.types ); - params = d.convertTypes( fs.types ); + TokenStruct fs = f.parseTypedList(true, d.types); + params = d.convertTypes(fs.types); - TokenStruct< std::string > fstruct( ts ); - fstruct.append( fs ); + TokenStruct fstruct(ts); + fstruct.append(fs); - f.next(); - f.assert_token( "(" ); - if ( f.getChar() != ')' ) { - cond = d.createCondition( f ); - cond->parse( f, fstruct, d ); - } - else ++f.c; + f.next(); + f.assert_token("("); + if (f.getChar() != ')') { + cond = d.createCondition(f); + cond->parse(f, fstruct, d); + } else + ++f.c; - f.next(); - f.assert_token( ")" ); + f.next(); + f.assert_token(")"); } -} } // namespaces +} // namespace pddl +} // namespace parser diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/Function.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/Function.cpp index 7f15b19f..c09de48c 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/Function.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/Function.cpp @@ -1,29 +1,37 @@ #include "plansys2_pddl_parser/Domain.h" -namespace parser { namespace pddl { - -void Function::PDDLPrint( std::ostream & s, unsigned indent, const TokenStruct< std::string > & ts, const Domain & d ) const { - Lifted::PDDLPrint( s, indent, ts, d ); - if ( returnType >= 0 ) s << " - " << d.types[returnType]->name; +namespace parser +{ +namespace pddl +{ +void Function::PDDLPrint( + std::ostream & s, unsigned indent, const TokenStruct & ts, const Domain & d) const +{ + Lifted::PDDLPrint(s, indent, ts, d); + if (returnType >= 0) s << " - " << d.types[returnType]->name; } -plansys2_msgs::msg::Node::SharedPtr Function::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace ) const { - throw UnsupportedConstruct("Function"); +plansys2_msgs::msg::Node::SharedPtr Function::getTree( + plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace) const +{ + throw UnsupportedConstruct("Function"); } -void Function::parse( Stringreader & f, TokenStruct< std::string > & ts, Domain & d ) { - Lifted::parse( f, ts, d ); - - f.next(); - if ( f.getChar() == '-' ) { - f.assert_token( "-" ); - std::string s = f.getToken(); - if ( s != "number" ) { - f.c -= s.size(); - returnType = d.types.index( f.getToken( d.types ) ); - } - } +void Function::parse(Stringreader & f, TokenStruct & ts, Domain & d) +{ + Lifted::parse(f, ts, d); + + f.next(); + if (f.getChar() == '-') { + f.assert_token("-"); + std::string s = f.getToken(); + if (s != "number") { + f.c -= s.size(); + returnType = d.types.index(f.getToken(d.types)); + } + } } -} } // namespaces +} // namespace pddl +} // namespace parser diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/FunctionModifier.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/FunctionModifier.cpp index 59889ba1..a0db269d 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/FunctionModifier.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/FunctionModifier.cpp @@ -1,87 +1,96 @@ #include "plansys2_pddl_parser/Domain.h" -namespace parser { namespace pddl { - -FunctionModifier::FunctionModifier( const std::string& name, int val ) - : name( name ), modifiedGround( 0 ), modifierExpr( new ValueExpression( val ) ) {} +namespace parser +{ +namespace pddl +{ +FunctionModifier::FunctionModifier(const std::string & name, int val) +: name(name), modifiedGround(0), modifierExpr(new ValueExpression(val)) +{ +} -FunctionModifier::FunctionModifier( const std::string& name, Function * f, const IntVec & p ) - : name( name ), modifiedGround( 0 ), modifierExpr( new FunctionExpression( new Ground( f, p ) ) ) {} +FunctionModifier::FunctionModifier(const std::string & name, Function * f, const IntVec & p) +: name(name), modifiedGround(0), modifierExpr(new FunctionExpression(new Ground(f, p))) +{ +} -FunctionModifier::FunctionModifier( const std::string& name, const FunctionModifier * i, Domain & d ) - : name( name ) +FunctionModifier::FunctionModifier(const std::string & name, const FunctionModifier * i, Domain & d) +: name(name) { - if ( i->modifiedGround ) { - modifiedGround = dynamic_cast< Ground * >( i->modifiedGround->copy( d ) ); - } - else modifiedGround = 0; - - if ( i->modifierExpr ) { - modifierExpr = dynamic_cast< Expression * >( i->modifierExpr->copy( d ) ); - } - else modifierExpr = 0; + if (i->modifiedGround) { + modifiedGround = dynamic_cast(i->modifiedGround->copy(d)); + } else + modifiedGround = 0; + + if (i->modifierExpr) { + modifierExpr = dynamic_cast(i->modifierExpr->copy(d)); + } else + modifierExpr = 0; } -void FunctionModifier::PDDLPrint( std::ostream & s, unsigned indent, const TokenStruct< std::string > & ts, const Domain & d ) const { - tabindent( s, indent ); - s << "( " << name << " "; +void FunctionModifier::PDDLPrint( + std::ostream & s, unsigned indent, const TokenStruct & ts, const Domain & d) const +{ + tabindent(s, indent); + s << "( " << name << " "; - if ( modifiedGround ) { - modifiedGround->PDDLPrint( s, 0, ts, d ); - } - else { - s << "( total-cost )"; - } + if (modifiedGround) { + modifiedGround->PDDLPrint(s, 0, ts, d); + } else { + s << "( total-cost )"; + } - s << " "; - modifierExpr->PDDLPrint( s, 0, ts, d ); + s << " "; + modifierExpr->PDDLPrint(s, 0, ts, d); - s << " )"; + s << " )"; } -plansys2_msgs::msg::Node::SharedPtr FunctionModifier::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace ) const { - plansys2_msgs::msg::Node::SharedPtr node = std::make_shared(); - node->node_type = plansys2_msgs::msg::Node::FUNCTION_MODIFIER; - node->modifier_type = getFunModType(name); - node->node_id = tree.nodes.size(); - tree.nodes.push_back(*node); - - if (modifiedGround) { - plansys2_msgs::msg::Node::SharedPtr child = modifiedGround->getTree(tree, d, replace); - tree.nodes[node->node_id].children.push_back(child->node_id); - } - else { - std::cerr << "function modifier for total-cost not supported" << std::endl; - } - - plansys2_msgs::msg::Node::SharedPtr child = modifierExpr->getTree(tree, d, replace); +plansys2_msgs::msg::Node::SharedPtr FunctionModifier::getTree( + plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace) const +{ + plansys2_msgs::msg::Node::SharedPtr node = std::make_shared(); + node->node_type = plansys2_msgs::msg::Node::FUNCTION_MODIFIER; + node->modifier_type = getFunModType(name); + node->node_id = tree.nodes.size(); + tree.nodes.push_back(*node); + + if (modifiedGround) { + plansys2_msgs::msg::Node::SharedPtr child = modifiedGround->getTree(tree, d, replace); tree.nodes[node->node_id].children.push_back(child->node_id); + } else { + std::cerr << "function modifier for total-cost not supported" << std::endl; + } - return node; + plansys2_msgs::msg::Node::SharedPtr child = modifierExpr->getTree(tree, d, replace); + tree.nodes[node->node_id].children.push_back(child->node_id); + + return node; } -void FunctionModifier::parse( Stringreader & f, TokenStruct< std::string > & ts, Domain & d ) { - f.next(); +void FunctionModifier::parse(Stringreader & f, TokenStruct & ts, Domain & d) +{ + f.next(); - f.assert_token( "(" ); + f.assert_token("("); - std::string increasedFunction = f.getToken(); - if ( increasedFunction == "total-cost" ) { - f.next(); - f.assert_token( ")" ); - } - else { - modifiedGround = new Ground( d.funcs.get( increasedFunction ) ); - modifiedGround->parse( f, ts, d ); - } + std::string increasedFunction = f.getToken(); + if (increasedFunction == "total-cost") { + f.next(); + f.assert_token(")"); + } else { + modifiedGround = new Ground(d.funcs.get(increasedFunction)); + modifiedGround->parse(f, ts, d); + } - f.next(); + f.next(); - modifierExpr = createExpression( f, ts, d ); + modifierExpr = createExpression(f, ts, d); - f.next(); - f.assert_token( ")" ); + f.next(); + f.assert_token(")"); } -} } // namespaces +} // namespace pddl +} // namespace parser diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/Ground.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/Ground.cpp index e274ebd7..eed7ace3 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/Ground.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/Ground.cpp @@ -1,92 +1,101 @@ -#include "plansys2_pddl_parser/Domain.h" #include -namespace parser { namespace pddl { +#include "plansys2_pddl_parser/Domain.h" -Ground::Ground( const Ground * g, Domain & d ) - : ParamCond( g ), lifted( d.preds.get( g->name ) ) {} +namespace parser +{ +namespace pddl +{ +Ground::Ground(const Ground * g, Domain & d) : ParamCond(g), lifted(d.preds.get(g->name)) {} -void Ground::PDDLPrint( std::ostream & s, unsigned indent, const TokenStruct< std::string > & ts, const Domain & d ) const { - tabindent( s, indent ); - s << "( " << name; - for ( unsigned i = 0; i < params.size(); ++i ) { - if ( false && ts.size() && params[i] >= 0 && (unsigned)params[i] < ts.size() ) { - s << " " << ts[params[i]]; - } else if (params[i] >= 0 && - d.types[lifted->params[i]]->object( params[i] ).first != std::string("")) { - s << " " << d.types[lifted->params[i]]->object( params[i] ).first; - } else if (params[i] < 0){ - int type_idx = lifted->params[i]; // idx of the type of this param [ref: d.type] - int constant_idx = (-1 * params[i]) -1; // idx of the constant value [ref: d.type.constant] - s << " " << d.types[type_idx]->constants[constant_idx]; // the actual constant value - } else { - s << " ?" + std::to_string(params[i]); - } - } - s << " )"; +void Ground::PDDLPrint( + std::ostream & s, unsigned indent, const TokenStruct & ts, const Domain & d) const +{ + tabindent(s, indent); + s << "( " << name; + for (unsigned i = 0; i < params.size(); ++i) { + if (false && ts.size() && params[i] >= 0 && (unsigned)params[i] < ts.size()) { + s << " " << ts[params[i]]; + } else if ( + params[i] >= 0 && d.types[lifted->params[i]]->object(params[i]).first != std::string("")) { + s << " " << d.types[lifted->params[i]]->object(params[i]).first; + } else if (params[i] < 0) { + int type_idx = lifted->params[i]; // idx of the type of this param [ref: d.type] + int constant_idx = (-1 * params[i]) - 1; // idx of the constant value [ref: d.type.constant] + s << " " << d.types[type_idx]->constants[constant_idx]; // the actual constant value + } else { + s << " ?" + std::to_string(params[i]); + } + } + s << " )"; } -plansys2_msgs::msg::Node::SharedPtr Ground::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace ) const { - plansys2_msgs::msg::Node::SharedPtr node = std::make_shared(); - if ( d.funcs.index( name ) >= 0) { - node->node_type = plansys2_msgs::msg::Node::FUNCTION; +plansys2_msgs::msg::Node::SharedPtr Ground::getTree( + plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace) const +{ + plansys2_msgs::msg::Node::SharedPtr node = std::make_shared(); + if (d.funcs.index(name) >= 0) { + node->node_type = plansys2_msgs::msg::Node::FUNCTION; + } else { + node->node_type = plansys2_msgs::msg::Node::PREDICATE; + } + node->node_id = tree.nodes.size(); + node->name = name; + for (unsigned i = 0; i < params.size(); ++i) { + plansys2_msgs::msg::Param param; + if (i < replace.size()) { + if (params[i] >= 0) { + // param has a variable value; replace by action-args + param.name = replace[params[i]]; + } else { // param has a constant value; retrieve from domain::type[t_i]::constants[c_i] + int type_idx = lifted->params[i]; // idx of the type of this param [ref: d.type] + int constant_idx = + (-1 * params[i]) - 1; // idx of the constant value [ref: d.type.constant] + param.name = d.types[type_idx]->constants[constant_idx]; // the actual constant value + } + } else if ( + params[i] >= 0 && d.types[lifted->params[i]]->object(params[i]).first != std::string("")) { + std::pair c = d.types[lifted->params[i]]->object(params[i]); + param.name = d.types[lifted->params[i]]->object(params[i]).first; + } else if (params[i] < 0) { + int type_idx = lifted->params[i]; // idx of the type of this param [ref: d.type] + int constant_idx = (-1 * params[i]) - 1; // idx of the constant value [ref: d.type.constant] + param.name = d.types[type_idx]->constants[constant_idx]; // the actual constant value } else { - node->node_type = plansys2_msgs::msg::Node::PREDICATE; - } - node->node_id = tree.nodes.size(); - node->name = name; - for ( unsigned i = 0; i < params.size(); ++i ) { - plansys2_msgs::msg::Param param; - if (i < replace.size()) { - if (params[i] >= 0) { - // param has a variable value; replace by action-args - param.name = replace[params[i]]; - } else { // param has a constant value; retrive from domain::type[t_i]::constants[c_i] - int type_idx = lifted->params[i]; // idx of the type of this param [ref: d.type] - int constant_idx = (-1 * params[i]) -1; // idx of the constant value [ref: d.type.constant] - param.name = d.types[type_idx]->constants[constant_idx]; // the actual constant value - } - } else if (params[i]>= 0 && - d.types[lifted->params[i]]->object( params[i] ).first != std::string("")) { - std::pair< std::string, int > c = d.types[lifted->params[i]]->object( params[i] ); - param.name = d.types[lifted->params[i]]->object( params[i] ).first; - } else if (params[i] < 0){ - int type_idx = lifted->params[i]; // idx of the type of this param [ref: d.type] - int constant_idx = (-1 * params[i]) -1; // idx of the constant value [ref: d.type.constant] - param.name = d.types[type_idx]->constants[constant_idx]; // the actual constant value - } else { - param.name = "?" + std::to_string(params[i]); - } - param.type = d.types[lifted->params[i]]->name; - node->parameters.push_back(param); + param.name = "?" + std::to_string(params[i]); } - tree.nodes.push_back(*node); - return node; + param.type = d.types[lifted->params[i]]->name; + node->parameters.push_back(param); + } + tree.nodes.push_back(*node); + return node; } -void Ground::parse( Stringreader & f, TokenStruct< std::string > & ts, Domain & d ) { - f.next(); - params.resize( lifted->params.size() ); - for ( unsigned i = 0; i < lifted->params.size(); ++i, f.next() ) { - std::string s = f.getToken(); - std::pair< bool, int > p = d.types[lifted->params[i]]->parseConstant( s ); - if ( p.first ) { - params[i] = p.second; - } else { - std::pair< bool, unsigned > q = d.types[lifted->params[i]]->parseObject( s ); - if ( q.first ) { - params[i] = q.second; +void Ground::parse(Stringreader & f, TokenStruct & ts, Domain & d) +{ + f.next(); + params.resize(lifted->params.size()); + for (unsigned i = 0; i < lifted->params.size(); ++i, f.next()) { + std::string s = f.getToken(); + std::pair p = d.types[lifted->params[i]]->parseConstant(s); + if (p.first) { + params[i] = p.second; } else { - int k = ts.index( s ); - if ( k >= 0 ) { - params[i] = k; + std::pair q = d.types[lifted->params[i]]->parseObject(s); + if (q.first) { + params[i] = q.second; + } else { + int k = ts.index(s); + if (k >= 0) { + params[i] = k; + } else + f.tokenExit(s); } - else f.tokenExit(s); } } - } - f.assert_token( ")" ); + f.assert_token(")"); } -} } // namespaces +} // namespace pddl +} // namespace parser diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/GroundFunc.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/GroundFunc.cpp index 04bc1157..020add9f 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/GroundFunc.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/GroundFunc.cpp @@ -3,67 +3,83 @@ #include "plansys2_pddl_parser/Domain.h" -namespace parser { namespace pddl { - +namespace parser +{ +namespace pddl +{ template <> -void GroundFunc::PDDLPrint( std::ostream & s, unsigned indent, const TokenStruct< std::string > & ts, const Domain & d ) const { - tabindent( s, indent ); - s << "( = "; - TypeGround::PDDLPrint( s, 0, ts, d ); - s << " " << std::fixed << std::setprecision(10) << ( double )value << " )"; +void GroundFunc::PDDLPrint( + std::ostream & s, unsigned indent, const TokenStruct & ts, const Domain & d) const +{ + tabindent(s, indent); + s << "( = "; + TypeGround::PDDLPrint(s, 0, ts, d); + s << " " << std::fixed << std::setprecision(10) << (double)value << " )"; } template <> -void GroundFunc::PDDLPrint( std::ostream & s, unsigned indent, const TokenStruct< std::string > & ts, const Domain & d ) const { - tabindent( s, indent ); - s << "( = "; - TypeGround::PDDLPrint( s, 0, ts, d ); - s << " " << d.types[((Function *)lifted)->returnType]->object( value ) << " )"; +void GroundFunc::PDDLPrint( + std::ostream & s, unsigned indent, const TokenStruct & ts, const Domain & d) const +{ + tabindent(s, indent); + s << "( = "; + TypeGround::PDDLPrint(s, 0, ts, d); + s << " " << d.types[((Function *)lifted)->returnType]->object(value) << " )"; } template <> -plansys2_msgs::msg::Node::SharedPtr GroundFunc::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace ) const { - auto node = TypeGround::getTree(tree, d, replace); - node->value = value; - return node; +plansys2_msgs::msg::Node::SharedPtr GroundFunc::getTree( + plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace) const +{ + auto node = TypeGround::getTree(tree, d, replace); + node->value = value; + return node; } template <> -plansys2_msgs::msg::Node::SharedPtr GroundFunc::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace ) const { - auto node = TypeGround::getTree(tree, d, replace); - node->value = value; - return node; +plansys2_msgs::msg::Node::SharedPtr GroundFunc::getTree( + plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace) const +{ + auto node = TypeGround::getTree(tree, d, replace); + node->value = value; + return node; } template <> -void GroundFunc::parse( Stringreader & f, TokenStruct< std::string > & ts, Domain & d ) { - TypeGround::parse( f, ts, d ); - - f.next(); - std::string s = f.getToken(); - std::istringstream i( s ); - if ( !( i >> value ) ) f.tokenExit( s ); +void GroundFunc::parse(Stringreader & f, TokenStruct & ts, Domain & d) +{ + TypeGround::parse(f, ts, d); + + f.next(); + std::string s = f.getToken(); + std::istringstream i(s); + if (!(i >> value)) f.tokenExit(s); - f.next(); - f.assert_token( ")" ); + f.next(); + f.assert_token(")"); } template <> -void GroundFunc::parse( Stringreader & f, TokenStruct< std::string > & ts, Domain & d ) { - TypeGround::parse( f, ts, d ); - - f.next(); - std::string s = f.getToken(); - std::pair< bool, unsigned > p = d.types[((Function *)lifted)->returnType]->parseObject( s ); - if ( p.first ) value = p.second; - else { - std::pair< bool, int > q = d.types[((Function *)lifted)->returnType]->parseConstant( s ); - if ( q.first ) value = q.second; - else f.tokenExit( s ); - } +void GroundFunc::parse(Stringreader & f, TokenStruct & ts, Domain & d) +{ + TypeGround::parse(f, ts, d); + + f.next(); + std::string s = f.getToken(); + std::pair p = d.types[((Function *)lifted)->returnType]->parseObject(s); + if (p.first) + value = p.second; + else { + std::pair q = d.types[((Function *)lifted)->returnType]->parseConstant(s); + if (q.first) + value = q.second; + else + f.tokenExit(s); + } - f.next(); - f.assert_token( ")" ); + f.next(); + f.assert_token(")"); } -} } // namespaces +} // namespace pddl +} // namespace parser diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/Lifted.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/Lifted.cpp index 59494a05..5e1935a8 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/Lifted.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/Lifted.cpp @@ -1,27 +1,36 @@ #include "plansys2_pddl_parser/Domain.h" -namespace parser { namespace pddl { - -void Lifted::PDDLPrint( std::ostream & s, unsigned indent, const TokenStruct< std::string > & ts, const Domain & d ) const { - tabindent( s, indent ); - s << "( " << name; - for ( unsigned i = 0; i < params.size(); ++i ) { - if ( ts.size() ) s << ts[i]; - else s << " ?" << d.types[params[i]]->getName() << i; - if ( d.typed ) s << " - " << d.types[params[i]]->name; - } - s << " )"; +namespace parser +{ +namespace pddl +{ +void Lifted::PDDLPrint( + std::ostream & s, unsigned indent, const TokenStruct & ts, const Domain & d) const +{ + tabindent(s, indent); + s << "( " << name; + for (unsigned i = 0; i < params.size(); ++i) { + if (ts.size()) + s << ts[i]; + else + s << " ?" << d.types[params[i]]->getName() << i; + if (d.typed) s << " - " << d.types[params[i]]->name; + } + s << " )"; } -plansys2_msgs::msg::Node::SharedPtr Lifted::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace ) const { - throw UnsupportedConstruct("Lifted"); +plansys2_msgs::msg::Node::SharedPtr Lifted::getTree( + plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace) const +{ + throw UnsupportedConstruct("Lifted"); } -void Lifted::parse( Stringreader & f, TokenStruct< std::string > & ts, Domain & d ) { - TokenStruct< std::string > lstruct = f.parseTypedList( true, d.types ); - params = d.convertTypes( lstruct.types ); +void Lifted::parse(Stringreader & f, TokenStruct & ts, Domain & d) +{ + TokenStruct lstruct = f.parseTypedList(true, d.types); + params = d.convertTypes(lstruct.types); } - -} } // namespaces +} // namespace pddl +} // namespace parser diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/Not.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/Not.cpp index 83d8d179..07e9dac9 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/Not.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/Not.cpp @@ -1,44 +1,51 @@ #include "plansys2_pddl_parser/Domain.h" -namespace parser { namespace pddl { - -void Not::PDDLPrint( std::ostream & s, unsigned indent, const TokenStruct< std::string > & ts, const Domain & d ) const { - tabindent( s, indent ); - s << "( not "; - if ( cond ) cond->PDDLPrint( s, 0, ts, d ); - s << " )"; +namespace parser +{ +namespace pddl +{ +void Not::PDDLPrint( + std::ostream & s, unsigned indent, const TokenStruct & ts, const Domain & d) const +{ + tabindent(s, indent); + s << "( not "; + if (cond) cond->PDDLPrint(s, 0, ts, d); + s << " )"; } -plansys2_msgs::msg::Node::SharedPtr Not::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace ) const { - plansys2_msgs::msg::Node::SharedPtr node = std::make_shared(); - node->node_type = plansys2_msgs::msg::Node::NOT; - node->node_id = tree.nodes.size(); - tree.nodes.push_back(*node); +plansys2_msgs::msg::Node::SharedPtr Not::getTree( + plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace) const +{ + plansys2_msgs::msg::Node::SharedPtr node = std::make_shared(); + node->node_type = plansys2_msgs::msg::Node::NOT; + node->node_id = tree.nodes.size(); + tree.nodes.push_back(*node); - if (cond) { - plansys2_msgs::msg::Node::SharedPtr child = cond->getTree(tree, d, replace); - tree.nodes[node->node_id].children.push_back(child->node_id); - } + if (cond) { + plansys2_msgs::msg::Node::SharedPtr child = cond->getTree(tree, d, replace); + tree.nodes[node->node_id].children.push_back(child->node_id); + } - return node; + return node; } -void Not::parse( Stringreader & f, TokenStruct< std::string > & ts, Domain & d ) { - f.next(); - f.assert_token( "(" ); +void Not::parse(Stringreader & f, TokenStruct & ts, Domain & d) +{ + f.next(); + f.assert_token("("); - cond = dynamic_cast< Ground * >( d.createCondition( f ) ); + cond = dynamic_cast(d.createCondition(f)); - if ( !cond ) { - f.tokenExit( f.getToken() ); - } + if (!cond) { + f.tokenExit(f.getToken()); + } - cond->parse( f, ts, d ); + cond->parse(f, ts, d); - f.next(); - f.assert_token( ")" ); + f.next(); + f.assert_token(")"); } - -} } // namespaces +} // namespace pddl +} // namespace parser diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/Oneof.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/Oneof.cpp index ba714ba2..6d169462 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/Oneof.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/Oneof.cpp @@ -1,31 +1,39 @@ #include "plansys2_pddl_parser/Domain.h" -namespace parser { namespace pddl { - -void Oneof::PDDLPrint( std::ostream & s, unsigned indent, const TokenStruct< std::string > & ts, const Domain & d ) const { - tabindent( s, indent ); - s << "( oneof\n"; - for ( unsigned i = 0; i < conds.size(); ++i ) { - conds[i]->PDDLPrint( s, indent + 1, ts, d ); - s << "\n"; - } - tabindent( s, indent ); - s << ")"; +namespace parser +{ +namespace pddl +{ +void Oneof::PDDLPrint( + std::ostream & s, unsigned indent, const TokenStruct & ts, const Domain & d) const +{ + tabindent(s, indent); + s << "( oneof\n"; + for (unsigned i = 0; i < conds.size(); ++i) { + conds[i]->PDDLPrint(s, indent + 1, ts, d); + s << "\n"; + } + tabindent(s, indent); + s << ")"; } -plansys2_msgs::msg::Node::SharedPtr Oneof::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace ) const { - throw UnsupportedConstruct("Oneof"); +plansys2_msgs::msg::Node::SharedPtr Oneof::getTree( + plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace) const +{ + throw UnsupportedConstruct("Oneof"); } -void Oneof::parse( Stringreader & f, TokenStruct< std::string > & ts, Domain & d ) { - for ( f.next(); f.getChar() != ')'; f.next() ) { - f.assert_token( "(" ); - Condition * condition = d.createCondition( f ); - condition->parse( f, ts, d ); - conds.push_back( condition ); - } - ++f.c; +void Oneof::parse(Stringreader & f, TokenStruct & ts, Domain & d) +{ + for (f.next(); f.getChar() != ')'; f.next()) { + f.assert_token("("); + Condition * condition = d.createCondition(f); + condition->parse(f, ts, d); + conds.push_back(condition); + } + ++f.c; } -} } // namespaces +} // namespace pddl +} // namespace parser diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/Or.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/Or.cpp index 365b30e1..59c3704f 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/Or.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/Or.cpp @@ -1,28 +1,36 @@ #include "plansys2_pddl_parser/Domain.h" -namespace parser { namespace pddl { - -void Or::PDDLPrint( std::ostream & s, unsigned indent, const TokenStruct< std::string > & ts, const Domain & d ) const { - tabindent( s, indent ); - s << "( or\n"; - if ( first ) first->PDDLPrint( s, indent + 1, ts, d ); - else { - tabindent( s, indent + 1 ); - s << "()"; - } - s << "\n"; - if ( second ) second->PDDLPrint( s, indent + 1, ts, d ); - else { - tabindent( s, indent + 1 ); - s << "()"; - } - s << "\n"; - tabindent( s, indent ); - s << ")"; +namespace parser +{ +namespace pddl +{ +void Or::PDDLPrint( + std::ostream & s, unsigned indent, const TokenStruct & ts, const Domain & d) const +{ + tabindent(s, indent); + s << "( or\n"; + if (first) + first->PDDLPrint(s, indent + 1, ts, d); + else { + tabindent(s, indent + 1); + s << "()"; + } + s << "\n"; + if (second) + second->PDDLPrint(s, indent + 1, ts, d); + else { + tabindent(s, indent + 1); + s << "()"; + } + s << "\n"; + tabindent(s, indent); + s << ")"; } -plansys2_msgs::msg::Node::SharedPtr Or::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace ) const { +plansys2_msgs::msg::Node::SharedPtr Or::getTree( + plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace) const +{ plansys2_msgs::msg::Node::SharedPtr node = std::make_shared(); node->node_type = plansys2_msgs::msg::Node::OR; node->node_id = tree.nodes.size(); @@ -36,26 +44,27 @@ plansys2_msgs::msg::Node::SharedPtr Or::getTree( plansys2_msgs::msg::Tree & tree return node; } -void Or::parse( Stringreader & f, TokenStruct< std::string > & ts, Domain & d ) { - f.next(); - f.assert_token( "(" ); - if ( f.getChar() != ')' ) { - first = d.createCondition( f ); - first->parse( f, ts, d ); - } - else ++f.c; - - f.next(); - f.assert_token( "(" ); - if ( f.getChar() != ')' ) { - second = d.createCondition( f ); - second->parse( f, ts, d ); - } - else ++f.c; - - f.next(); - f.assert_token( ")" ); -} +void Or::parse(Stringreader & f, TokenStruct & ts, Domain & d) +{ + f.next(); + f.assert_token("("); + if (f.getChar() != ')') { + first = d.createCondition(f); + first->parse(f, ts, d); + } else + ++f.c; + + f.next(); + f.assert_token("("); + if (f.getChar() != ')') { + second = d.createCondition(f); + second->parse(f, ts, d); + } else + ++f.c; + f.next(); + f.assert_token(")"); +} -} } // namespaces +} // namespace pddl +} // namespace parser diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/ParamCond.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/ParamCond.cpp index 0efd960c..5fe96f37 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/ParamCond.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/ParamCond.cpp @@ -1,18 +1,23 @@ #include "plansys2_pddl_parser/Domain.h" -namespace parser { namespace pddl { - -void ParamCond::printParams( unsigned first, std::ostream & s, TokenStruct< std::string > & ts, const Domain & d ) const { - s << "("; - for ( unsigned i = first; i < params.size(); ++i ) { - std::stringstream ss; - ss << "?" << d.types[params[i]]->getName() << ts.size(); - ts.insert( ss.str() ); - s << " " << ss.str(); - if ( d.typed ) s << " - " << d.types[params[i]]->name; - } - s << " )\n"; +namespace parser +{ +namespace pddl +{ +void ParamCond::printParams( + unsigned first, std::ostream & s, TokenStruct & ts, const Domain & d) const +{ + s << "("; + for (unsigned i = first; i < params.size(); ++i) { + std::stringstream ss; + ss << "?" << d.types[params[i]]->getName() << ts.size(); + ts.insert(ss.str()); + s << " " << ss.str(); + if (d.typed) s << " - " << d.types[params[i]]->name; + } + s << " )\n"; } -} } // namespaces +} // namespace pddl +} // namespace parser diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/TemporalAction.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/TemporalAction.cpp index 0b9e787f..c9290f7f 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/TemporalAction.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/TemporalAction.cpp @@ -1,202 +1,208 @@ #include "plansys2_pddl_parser/Instance.h" -namespace parser { namespace pddl { - -Expression * TemporalAction::parseDuration( Stringreader & f, TokenStruct< std::string > & ts, Domain & d ) { - return createExpression( f, ts, d ); +namespace parser +{ +namespace pddl +{ +Expression * TemporalAction::parseDuration( + Stringreader & f, TokenStruct & ts, Domain & d) +{ + return createExpression(f, ts, d); } -void TemporalAction::printCondition( std::ostream & s, const TokenStruct< std::string > & ts, const Domain & d, - const std::string & t, And * a ) const { - for ( unsigned i = 0; a && i < a->conds.size(); ++i ) { - s << "\t\t( " << t << " "; - a->conds[i]->PDDLPrint( s, 0, ts, d ); - s << " )\n"; - } +void TemporalAction::printCondition( + std::ostream & s, const TokenStruct & ts, const Domain & d, const std::string & t, + And * a) const +{ + for (unsigned i = 0; a && i < a->conds.size(); ++i) { + s << "\t\t( " << t << " "; + a->conds[i]->PDDLPrint(s, 0, ts, d); + s << " )\n"; + } } -void TemporalAction::PDDLPrint( std::ostream & s, unsigned indent, const TokenStruct< std::string > & ts, const Domain & d ) const { - s << "( :durative-action " << name << "\n"; +void TemporalAction::PDDLPrint( + std::ostream & s, unsigned indent, const TokenStruct & ts, const Domain & d) const +{ + s << "( :durative-action " << name << "\n"; - s << " :parameters "; + s << " :parameters "; - TokenStruct< std::string > astruct; + TokenStruct astruct; - printParams( 0, s, astruct, d ); + printParams(0, s, astruct, d); - s << " :duration ( = ?duration "; - if ( durationExpr ) durationExpr->PDDLPrint( s, 0, astruct, d ); - else s << "1"; - s << " )\n"; + s << " :duration ( = ?duration "; + if (durationExpr) + durationExpr->PDDLPrint(s, 0, astruct, d); + else + s << "1"; + s << " )\n"; - s << " :condition\n"; - s << "\t( and\n"; - printCondition( s, astruct, d, "at start", (And *)pre ); - printCondition( s, astruct, d, "over all", pre_o ); - printCondition( s, astruct, d, "at end", pre_e ); - s << "\t)\n"; + s << " :condition\n"; + s << "\t( and\n"; + printCondition(s, astruct, d, "at start", (And *)pre); + printCondition(s, astruct, d, "over all", pre_o); + printCondition(s, astruct, d, "at end", pre_e); + s << "\t)\n"; - s << " :effect\n"; - s << "\t( and\n"; - printCondition( s, astruct, d, "at start", (And *)eff ); - printCondition( s, astruct, d, "at end", eff_e ); - s << "\t)\n"; + s << " :effect\n"; + s << "\t( and\n"; + printCondition(s, astruct, d, "at start", (And *)eff); + printCondition(s, astruct, d, "at end", eff_e); + s << "\t)\n"; - s << ")\n"; + s << ")\n"; } -plansys2_msgs::msg::Node::SharedPtr TemporalAction::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace ) const { - throw UnsupportedConstruct("TemporalAction"); +plansys2_msgs::msg::Node::SharedPtr TemporalAction::getTree( + plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace) const +{ + throw UnsupportedConstruct("TemporalAction"); } -void TemporalAction::parseCondition( Stringreader & f, TokenStruct< std::string > & ts, Domain & d, And * a ) { - f.next(); - f.assert_token( "(" ); - Condition * c = d.createCondition( f ); - c->parse( f, ts, d ); - a->conds.push_back( c ); +void TemporalAction::parseCondition( + Stringreader & f, TokenStruct & ts, Domain & d, And * a) +{ + f.next(); + f.assert_token("("); + Condition * c = d.createCondition(f); + c->parse(f, ts, d); + a->conds.push_back(c); } -void TemporalAction::parse( Stringreader & f, TokenStruct< std::string > & ts, Domain & d ) { - f.next(); - f.assert_token( ":parameters" ); - f.assert_token( "(" ); - - TokenStruct< std::string > astruct = f.parseTypedList( true, d.types ); - params = d.convertTypes( astruct.types ); - - f.next(); - f.assert_token( ":duration" ); - f.assert_token( "(" ); - f.assert_token( "=" ); - f.assert_token( "?duration" ); - durationExpr = parseDuration( f, astruct, d ); - f.next(); - f.assert_token( ")" ); - - f.next(); - f.assert_token( ":" ); - std::string s = f.getToken(); - if ( s == "condition" ) { - pre = new And; - pre_o = new And; - pre_e = new And; - f.next(); - f.assert_token( "(" ); - if ( f.getChar() != ')' ) { - s = f.getToken(); - if ( s == "and" ) { - for ( f.next(); f.getChar() != ')'; f.next() ) { - f.assert_token( "(" ); - s = f.getToken(); - f.next(); - std::string t = f.getToken(); - - if ( s == "at" && t == "start" ) - parseCondition( f, astruct, d, (And *)pre ); - else if ( s == "over" && t == "all" ) - parseCondition( f, astruct, d, pre_o ); - else if ( s == "at" && t == "end" ) - parseCondition( f, astruct, d, pre_e ); - else f.tokenExit( s + " " + t ); - - f.next(); - f.assert_token( ")" ); - } - ++f.c; - } - else { - f.next(); - std::string t = f.getToken(); - - if ( s == "at" && t == "start" ) - parseCondition( f, astruct, d, (And *)pre ); - else if ( s == "over" && t == "all" ) - parseCondition( f, astruct, d, pre_o ); - else if ( s == "at" && t == "end" ) - parseCondition( f, astruct, d, pre_e ); - else f.tokenExit( s + " " + t ); - - f.next(); - f.assert_token( ")" ); - } - } - else ++f.c; - - f.next(); - f.assert_token( ":" ); - s = f.getToken(); - } - if ( s != "effect" ) f.tokenExit( s ); - - f.next(); - f.assert_token( "(" ); - if ( f.getChar() != ')' ) { - eff = new And; - eff_e = new And; - - s = f.getToken(); - if ( s == "and" ) { - for ( f.next(); f.getChar() != ')'; f.next() ) { - f.assert_token( "(" ); - s = f.getToken(); - f.next(); - std::string t = f.getToken(); - - if ( s == "at" && t == "start" ) - parseCondition( f, astruct, d, (And *)eff ); - else if ( s == "at" && t == "end" ) - parseCondition( f, astruct, d, eff_e ); - else f.tokenExit( s + " " + t ); - - f.next(); - f.assert_token( ")" ); - } - ++f.c; - } - else { - f.next(); - std::string t = f.getToken(); - - if ( s == "at" && t == "start" ) - parseCondition( f, astruct, d, (And *)eff ); - else if ( s == "at" && t == "end" ) - parseCondition( f, astruct, d, eff_e ); - else f.tokenExit( s + " " + t ); - - f.next(); - f.assert_token( ")" ); - } - } - else ++f.c; - - f.next(); - f.assert_token( ")" ); +void TemporalAction::parse(Stringreader & f, TokenStruct & ts, Domain & d) +{ + f.next(); + f.assert_token(":parameters"); + f.assert_token("("); + + TokenStruct astruct = f.parseTypedList(true, d.types); + params = d.convertTypes(astruct.types); + + f.next(); + f.assert_token(":duration"); + f.assert_token("("); + f.assert_token("="); + f.assert_token("?duration"); + durationExpr = parseDuration(f, astruct, d); + f.next(); + f.assert_token(")"); + + f.next(); + f.assert_token(":"); + std::string s = f.getToken(); + if (s == "condition") { + pre = new And; + pre_o = new And; + pre_e = new And; + f.next(); + f.assert_token("("); + if (f.getChar() != ')') { + s = f.getToken(); + if (s == "and") { + for (f.next(); f.getChar() != ')'; f.next()) { + f.assert_token("("); + s = f.getToken(); + f.next(); + std::string t = f.getToken(); + + if (s == "at" && t == "start") + parseCondition(f, astruct, d, (And *)pre); + else if (s == "over" && t == "all") + parseCondition(f, astruct, d, pre_o); + else if (s == "at" && t == "end") + parseCondition(f, astruct, d, pre_e); + else + f.tokenExit(s + " " + t); + + f.next(); + f.assert_token(")"); + } + ++f.c; + } else { + f.next(); + std::string t = f.getToken(); + + if (s == "at" && t == "start") + parseCondition(f, astruct, d, (And *)pre); + else if (s == "over" && t == "all") + parseCondition(f, astruct, d, pre_o); + else if (s == "at" && t == "end") + parseCondition(f, astruct, d, pre_e); + else + f.tokenExit(s + " " + t); + + f.next(); + f.assert_token(")"); + } + } else + ++f.c; + + f.next(); + f.assert_token(":"); + s = f.getToken(); + } + if (s != "effect") f.tokenExit(s); + + f.next(); + f.assert_token("("); + if (f.getChar() != ')') { + eff = new And; + eff_e = new And; + + s = f.getToken(); + if (s == "and") { + for (f.next(); f.getChar() != ')'; f.next()) { + f.assert_token("("); + s = f.getToken(); + f.next(); + std::string t = f.getToken(); + + if (s == "at" && t == "start") + parseCondition(f, astruct, d, (And *)eff); + else if (s == "at" && t == "end") + parseCondition(f, astruct, d, eff_e); + else + f.tokenExit(s + " " + t); + + f.next(); + f.assert_token(")"); + } + ++f.c; + } else { + f.next(); + std::string t = f.getToken(); + + if (s == "at" && t == "start") + parseCondition(f, astruct, d, (And *)eff); + else if (s == "at" && t == "end") + parseCondition(f, astruct, d, eff_e); + else + f.tokenExit(s + " " + t); + + f.next(); + f.assert_token(")"); + } + } else + ++f.c; + + f.next(); + f.assert_token(")"); } -GroundVec TemporalAction::preconsStart() { - return getGroundsFromCondition( pre, false ); -} +GroundVec TemporalAction::preconsStart() { return getGroundsFromCondition(pre, false); } -GroundVec TemporalAction::preconsOverall() { - return getGroundsFromCondition( pre_o, false ); -} +GroundVec TemporalAction::preconsOverall() { return getGroundsFromCondition(pre_o, false); } -GroundVec TemporalAction::preconsEnd() { - return getGroundsFromCondition( pre_e, false ); -} +GroundVec TemporalAction::preconsEnd() { return getGroundsFromCondition(pre_e, false); } -CondVec TemporalAction::endEffects() { - return getSubconditionsFromCondition( eff_e ); -} +CondVec TemporalAction::endEffects() { return getSubconditionsFromCondition(eff_e); } -GroundVec TemporalAction::addEndEffects() { - return getGroundsFromCondition( eff_e, false ); -} +GroundVec TemporalAction::addEndEffects() { return getGroundsFromCondition(eff_e, false); } -GroundVec TemporalAction::deleteEndEffects() { - return getGroundsFromCondition( eff_e, true ); -} +GroundVec TemporalAction::deleteEndEffects() { return getGroundsFromCondition(eff_e, true); } -} } // namespaces +} // namespace pddl +} // namespace parser diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/TypeGround.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/TypeGround.cpp index 3fd0037c..f03a2dbc 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/TypeGround.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/TypeGround.cpp @@ -1,50 +1,64 @@ #include "plansys2_pddl_parser/Domain.h" -namespace parser { namespace pddl { - -void TypeGround::PDDLPrint( std::ostream & s, unsigned indent, const TokenStruct< std::string > & ts, const Domain & d ) const { - tabindent( s, indent ); - s << "( " << name; - for ( unsigned i = 0; i < params.size(); ++i ) - s << " " << d.types[lifted->params[i]]->object( params[i] ).first; - s << " )"; +namespace parser +{ +namespace pddl +{ +void TypeGround::PDDLPrint( + std::ostream & s, unsigned indent, const TokenStruct & ts, const Domain & d) const +{ + tabindent(s, indent); + s << "( " << name; + for (unsigned i = 0; i < params.size(); ++i) + s << " " << d.types[lifted->params[i]]->object(params[i]).first; + s << " )"; } -plansys2_msgs::msg::Node::SharedPtr TypeGround::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace ) const { - return Ground::getTree(tree, d, replace); +plansys2_msgs::msg::Node::SharedPtr TypeGround::getTree( + plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace) const +{ + return Ground::getTree(tree, d, replace); } -void TypeGround::insert( Domain & d, const StringVec & v ) { - params.resize( lifted->params.size() ); - for ( unsigned i = 0; i < lifted->params.size(); ++i ) { - std::pair< bool, unsigned > p = d.types[lifted->params[i]]->parseObject( v[i] ); - if ( p.first ) params[i] = p.second; - else { - std::pair< bool, int > q = d.types[lifted->params[i]]->parseConstant( v[i] ); - if ( q.first ) params[i] = q.second; - else { - std::cerr << "Unknown object " << v[i] << "\n"; - std::exit( 1 ); - } - } - } +void TypeGround::insert(Domain & d, const StringVec & v) +{ + params.resize(lifted->params.size()); + for (unsigned i = 0; i < lifted->params.size(); ++i) { + std::pair p = d.types[lifted->params[i]]->parseObject(v[i]); + if (p.first) + params[i] = p.second; + else { + std::pair q = d.types[lifted->params[i]]->parseConstant(v[i]); + if (q.first) + params[i] = q.second; + else { + std::cerr << "Unknown object " << v[i] << "\n"; + std::exit(1); + } + } + } } -void TypeGround::parse( Stringreader & f, TokenStruct< std::string > & ts, Domain & d ) { - f.next(); - params.resize( lifted->params.size() ); - for ( unsigned i = 0; i < lifted->params.size(); ++i, f.next() ) { - std::string s = f.getToken(); - std::pair< bool, unsigned > p = d.types[lifted->params[i]]->parseObject( s ); - if ( p.first ) params[i] = p.second; - else { - std::pair< bool, int > q = d.types[lifted->params[i]]->parseConstant( s ); - if ( q.first ) params[i] = q.second; - else f.tokenExit( s ); - } - } - f.assert_token( ")" ); +void TypeGround::parse(Stringreader & f, TokenStruct & ts, Domain & d) +{ + f.next(); + params.resize(lifted->params.size()); + for (unsigned i = 0; i < lifted->params.size(); ++i, f.next()) { + std::string s = f.getToken(); + std::pair p = d.types[lifted->params[i]]->parseObject(s); + if (p.first) + params[i] = p.second; + else { + std::pair q = d.types[lifted->params[i]]->parseConstant(s); + if (q.first) + params[i] = q.second; + else + f.tokenExit(s); + } + } + f.assert_token(")"); } -} } // namespaces +} // namespace pddl +} // namespace parser diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/Utils.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/Utils.cpp index ab0b297d..a9159a7c 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/Utils.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/Utils.cpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include -#include #include "plansys2_pddl_parser/Utils.h" +#include +#include +#include +#include + namespace parser { namespace pddl { - std::string getReducedString(const std::string & expr) { std::regex nts_chars("[\n\t]*", std::regex_constants::ECMAScript); @@ -102,8 +102,9 @@ uint8_t getNodeType(const std::string & expr, uint8_t default_node_type) } } - // The number search must precede the expression search in order to differentiate between an - // addition or subtraction expression and a number with a "+" or "-" prefix. + // The number search must precede the expression search in order to + // differentiate between an addition or subtraction expression and a number + // with a "+" or "-" prefix. std::tuple expression_search_result = getExpr(expr); if (std::get<0>(expression_search_result) != plansys2_msgs::msg::Node::UNKNOWN) { if (std::get<1>(expression_search_result) < first) { @@ -251,8 +252,12 @@ int getParenthesis(const std::string & wexpr, int start) int balance = 1; while (it < wexpr.size()) { - if (wexpr[it] == '(') {balance++;} - if (wexpr[it] == ')') {balance--;} + if (wexpr[it] == '(') { + balance++; + } + if (wexpr[it] == ')') { + balance--; + } if (balance == 0) { return it; @@ -271,9 +276,13 @@ std::vector getSubExpr(const std::string & expr) std::string wexpr(expr); // Remove first ( and last ) - while (wexpr.back() == ' ') {wexpr.pop_back();} + while (wexpr.back() == ' ') { + wexpr.pop_back(); + } wexpr.pop_back(); - while (wexpr.front() == ' ') {wexpr.erase(0, 1);} + while (wexpr.front() == ' ') { + wexpr.erase(0, 1); + } wexpr.erase(0, 1); while (wexpr.size() > 0) { @@ -367,8 +376,9 @@ std::string toString(const plansys2_msgs::msg::Tree & tree, uint32_t node_id, bo std::string toString(const plansys2_msgs::msg::Node & node) { - if (node.node_type != plansys2_msgs::msg::Node::PREDICATE && - node.node_type != plansys2_msgs::msg::Node::FUNCTION) { + if ( + node.node_type != plansys2_msgs::msg::Node::PREDICATE && + node.node_type != plansys2_msgs::msg::Node::FUNCTION) { std::cerr << "Unsupported node to string conversion" << std::endl; return {}; } @@ -550,7 +560,8 @@ std::string toStringExpression(const plansys2_msgs::msg::Tree & tree, uint32_t n return ret; } -std::string toStringFunctionModifier(const plansys2_msgs::msg::Tree & tree, uint32_t node_id, bool negate) +std::string toStringFunctionModifier( + const plansys2_msgs::msg::Tree & tree, uint32_t node_id, bool negate) { if (node_id >= tree.nodes.size()) { return {}; @@ -590,7 +601,8 @@ std::string toStringFunctionModifier(const plansys2_msgs::msg::Tree & tree, uint return ret; } -plansys2_msgs::msg::Node::SharedPtr fromString(plansys2_msgs::msg::Tree & tree, const std::string & expr, bool negate, uint8_t parent) +plansys2_msgs::msg::Node::SharedPtr fromString( + plansys2_msgs::msg::Tree & tree, const std::string & expr, bool negate, uint8_t parent) { std::string wexpr = getReducedString(expr); @@ -619,114 +631,114 @@ plansys2_msgs::msg::Node::SharedPtr fromString(plansys2_msgs::msg::Tree & tree, } switch (node_type) { case plansys2_msgs::msg::Node::AND: { - auto node = std::make_shared(); - node->node_type = node_type; - node->node_id = tree.nodes.size(); - node->negate = negate; - tree.nodes.push_back(*node); + auto node = std::make_shared(); + node->node_type = node_type; + node->node_id = tree.nodes.size(); + node->negate = negate; + tree.nodes.push_back(*node); - std::vector subexprs = getSubExpr(wexpr); - - for (const auto & e : subexprs) { - auto child = fromString(tree, e, negate, node_type); - tree.nodes[node->node_id].children.push_back(child->node_id); - } + std::vector subexprs = getSubExpr(wexpr); - return node; + for (const auto & e : subexprs) { + auto child = fromString(tree, e, negate, node_type); + tree.nodes[node->node_id].children.push_back(child->node_id); } - case plansys2_msgs::msg::Node::OR: { - auto node = std::make_shared(); - node->node_type = node_type; - node->node_id = tree.nodes.size(); - node->negate = negate; - tree.nodes.push_back(*node); - std::vector subexprs = getSubExpr(wexpr); + return node; + } + case plansys2_msgs::msg::Node::OR: { + auto node = std::make_shared(); + node->node_type = node_type; + node->node_id = tree.nodes.size(); + node->negate = negate; + tree.nodes.push_back(*node); - for (const auto & e : subexprs) { - auto child = fromString(tree, e, negate, node_type); - tree.nodes[node->node_id].children.push_back(child->node_id); - } + std::vector subexprs = getSubExpr(wexpr); - return node; + for (const auto & e : subexprs) { + auto child = fromString(tree, e, negate, node_type); + tree.nodes[node->node_id].children.push_back(child->node_id); } + + return node; + } case plansys2_msgs::msg::Node::NOT: { - auto node = std::make_shared(); - node->node_type = node_type; - node->node_id = tree.nodes.size(); - node->negate = negate; - tree.nodes.push_back(*node); + auto node = std::make_shared(); + node->node_type = node_type; + node->node_id = tree.nodes.size(); + node->negate = negate; + tree.nodes.push_back(*node); - std::vector subexprs = getSubExpr(wexpr); + std::vector subexprs = getSubExpr(wexpr); - auto child = fromString(tree, subexprs[0], !negate, node_type); - tree.nodes[node->node_id].children.push_back(child->node_id); + auto child = fromString(tree, subexprs[0], !negate, node_type); + tree.nodes[node->node_id].children.push_back(child->node_id); - return node; - } + return node; + } case plansys2_msgs::msg::Node::PREDICATE: { - auto node = std::make_shared(fromStringPredicate(wexpr)); - node->node_id = tree.nodes.size(); - node->negate = negate; - tree.nodes.push_back(*node); + auto node = std::make_shared(fromStringPredicate(wexpr)); + node->node_id = tree.nodes.size(); + node->negate = negate; + tree.nodes.push_back(*node); - return node; - } + return node; + } case plansys2_msgs::msg::Node::FUNCTION: { - auto node = std::make_shared(fromStringFunction(wexpr)); - node->node_id = tree.nodes.size(); - node->negate = negate; - tree.nodes.push_back(*node); + auto node = std::make_shared(fromStringFunction(wexpr)); + node->node_id = tree.nodes.size(); + node->negate = negate; + tree.nodes.push_back(*node); - return node; + return node; } case plansys2_msgs::msg::Node::EXPRESSION: { - auto node = std::make_shared(); - node->node_type = node_type; - node->expression_type = getExprType(wexpr); - node->node_id = tree.nodes.size(); - node->negate = negate; - tree.nodes.push_back(*node); - - std::vector subexprs = getSubExpr(wexpr); - - for (const auto & e : subexprs) { - auto child = fromString(tree, e, false, node_type); - tree.nodes[node->node_id].children.push_back(child->node_id); - } + auto node = std::make_shared(); + node->node_type = node_type; + node->expression_type = getExprType(wexpr); + node->node_id = tree.nodes.size(); + node->negate = negate; + tree.nodes.push_back(*node); + + std::vector subexprs = getSubExpr(wexpr); - return node; + for (const auto & e : subexprs) { + auto child = fromString(tree, e, false, node_type); + tree.nodes[node->node_id].children.push_back(child->node_id); + } + + return node; } case plansys2_msgs::msg::Node::FUNCTION_MODIFIER: { - auto node = std::make_shared(); - node->node_type = node_type; - node->modifier_type = getFunModType(wexpr); - node->node_id = tree.nodes.size(); - node->negate = negate; - tree.nodes.push_back(*node); - - std::vector subexprs = getSubExpr(wexpr); - - for (const auto & e : subexprs) { - auto child = fromString(tree, e, false, node_type); - tree.nodes[node->node_id].children.push_back(child->node_id); - } + auto node = std::make_shared(); + node->node_type = node_type; + node->modifier_type = getFunModType(wexpr); + node->node_id = tree.nodes.size(); + node->negate = negate; + tree.nodes.push_back(*node); + + std::vector subexprs = getSubExpr(wexpr); - return node; + for (const auto & e : subexprs) { + auto child = fromString(tree, e, false, node_type); + tree.nodes[node->node_id].children.push_back(child->node_id); + } + + return node; } case plansys2_msgs::msg::Node::NUMBER: { - auto node = std::make_shared(); - node->node_type = node_type; - node->node_id = tree.nodes.size(); - node->value = std::stod(wexpr); - node->negate = negate; - tree.nodes.push_back(*node); - - return node; + auto node = std::make_shared(); + node->node_type = node_type; + node->node_id = tree.nodes.size(); + node->value = std::stod(wexpr); + node->negate = negate; + tree.nodes.push_back(*node); + + return node; } // LCOV_EXCL_START default: - std::cerr << "fromString: Error parsing expresion [" << wexpr << "]" << std::endl; + std::cerr << "fromString: Error parsing expression [" << wexpr << "]" << std::endl; // LCOV_EXCL_STOP } @@ -751,9 +763,7 @@ plansys2_msgs::msg::Node fromStringPredicate(const std::string & predicate) while (end != std::string::npos) { end = predicate.find(" ", start); tokens.push_back( - predicate.substr( - start, - (end == std::string::npos) ? std::string::npos : end - start)); + predicate.substr(start, (end == std::string::npos) ? std::string::npos : end - start)); start = ((end > (std::string::npos - 1)) ? std::string::npos : end + 1); } @@ -832,16 +842,20 @@ plansys2_msgs::msg::Tree fromPredicates(const std::vector & preds) return tree; } -plansys2_msgs::msg::Tree::SharedPtr fromSubtree(const plansys2_msgs::msg::Tree & subtree, uint8_t node_type) +plansys2_msgs::msg::Tree::SharedPtr fromSubtree( + const plansys2_msgs::msg::Tree & subtree, uint8_t node_type) { std::vector temp; temp.push_back(subtree); return fromSubtrees(temp, node_type); } -plansys2_msgs::msg::Tree::SharedPtr fromSubtrees(const std::vector & subtrees, uint8_t node_type) +plansys2_msgs::msg::Tree::SharedPtr fromSubtrees( + const std::vector & subtrees, uint8_t node_type) { - if (node_type != plansys2_msgs::msg::Node::AND && node_type != plansys2_msgs::msg::Node::OR && node_type != plansys2_msgs::msg::Node::NOT) { + if ( + node_type != plansys2_msgs::msg::Node::AND && node_type != plansys2_msgs::msg::Node::OR && + node_type != plansys2_msgs::msg::Node::NOT) { std::cerr << "fromSubtrees: Unsupported root type." << std::endl; return nullptr; } @@ -867,7 +881,8 @@ plansys2_msgs::msg::Tree::SharedPtr fromSubtrees(const std::vectornodes.size(); tree->nodes[0].children.push_back(tree_size); - tree->nodes.insert(std::end(tree->nodes), std::begin(subtrees[i].nodes), std::end(subtrees[i].nodes)); + tree->nodes.insert( + std::end(tree->nodes), std::begin(subtrees[i].nodes), std::end(subtrees[i].nodes)); for (unsigned j = 0; j < subtrees[i].nodes.size(); ++j) { tree->nodes[tree_size + j].node_id += tree_size; for (unsigned k = 0; k < subtrees[i].nodes[j].children.size(); ++k) { @@ -891,10 +906,11 @@ std::vector getSubtreeIds(const plansys2_msgs::msg::Tree & tree) switch (tree.nodes.front().node_type) { case plansys2_msgs::msg::Node::AND: { - return tree.nodes.front().children; - } + return tree.nodes.front().children; + } default: - std::cerr << "getSubtreeIds: Error parsing expresion [" << toString(tree) << "]" << std::endl; + std::cerr << "getSubtreeIds: Error parsing expression [" << toString(tree) << "]" + << std::endl; } return {}; @@ -904,8 +920,7 @@ std::vector getSubtrees(const plansys2_msgs::msg::Tree { std::vector node_ids = parser::pddl::getSubtreeIds(tree); std::vector subtrees; - for (auto node_id : node_ids) - { + for (auto node_id : node_ids) { plansys2_msgs::msg::Tree subtree; subtree.nodes.push_back(tree.nodes[node_id]); subtree.nodes[0].node_id = 0; @@ -916,7 +931,9 @@ std::vector getSubtrees(const plansys2_msgs::msg::Tree return subtrees; } -void getSubtreeChildren(plansys2_msgs::msg::Tree & subtree, const plansys2_msgs::msg::Tree & tree, uint32_t tree_parent, uint32_t subtree_parent) +void getSubtreeChildren( + plansys2_msgs::msg::Tree & subtree, const plansys2_msgs::msg::Tree & tree, uint32_t tree_parent, + uint32_t subtree_parent) { for (auto child_id : tree.nodes[tree_parent].children) { auto subtree_size = subtree.nodes.size(); @@ -928,7 +945,9 @@ void getSubtreeChildren(plansys2_msgs::msg::Tree & subtree, const plansys2_msgs: } } -void getPredicates(std::vector & predicates, const plansys2_msgs::msg::Tree & tree, uint32_t node_id, bool negate) +void getPredicates( + std::vector & predicates, const plansys2_msgs::msg::Tree & tree, + uint32_t node_id, bool negate) { if (node_id >= tree.nodes.size()) { return; @@ -961,7 +980,10 @@ void getPredicates(std::vector & predicates, const pla break; case plansys2_msgs::msg::Node::PREDICATE: plansys2_msgs::msg::Node pred = tree.nodes[node_id]; - if (std::find_if(predicates.begin(), predicates.end(), std::bind(&checkNodeEquality, std::placeholders::_1, pred)) == predicates.end()) { + if ( + std::find_if( + predicates.begin(), predicates.end(), + std::bind(&checkNodeEquality, std::placeholders::_1, pred)) == predicates.end()) { pred.negate = negate; predicates.push_back(pred); } @@ -969,7 +991,9 @@ void getPredicates(std::vector & predicates, const pla } } -void getFunctions(std::vector & functions, const plansys2_msgs::msg::Tree & tree, uint32_t node_id, bool negate) +void getFunctions( + std::vector & functions, const plansys2_msgs::msg::Tree & tree, + uint32_t node_id, bool negate) { if (node_id >= tree.nodes.size()) { return; @@ -999,7 +1023,10 @@ void getFunctions(std::vector & functions, const plans break; case plansys2_msgs::msg::Node::FUNCTION: plansys2_msgs::msg::Node func = tree.nodes[node_id]; - if (std::find_if(functions.begin(), functions.end(), std::bind(&checkNodeEquality, std::placeholders::_1, func)) == functions.end()) { + if ( + std::find_if( + functions.begin(), functions.end(), + std::bind(&checkNodeEquality, std::placeholders::_1, func)) == functions.end()) { func.value = 0.0; functions.push_back(func); } @@ -1007,7 +1034,8 @@ void getFunctions(std::vector & functions, const plans } } -bool checkTreeEquality(const plansys2_msgs::msg::Tree & first, const plansys2_msgs::msg::Tree & second) +bool checkTreeEquality( + const plansys2_msgs::msg::Tree & first, const plansys2_msgs::msg::Tree & second) { if (first.nodes.size() != second.nodes.size()) { return false; @@ -1022,16 +1050,17 @@ bool checkTreeEquality(const plansys2_msgs::msg::Tree & first, const plansys2_ms return true; } -bool checkNodeEquality(const plansys2_msgs::msg::Node & first, const plansys2_msgs::msg::Node & second) +bool checkNodeEquality( + const plansys2_msgs::msg::Node & first, const plansys2_msgs::msg::Node & second) { if (first.node_type != second.node_type) { return false; } - if (first.node_type == plansys2_msgs::msg::Node::ACTION || - first.node_type == plansys2_msgs::msg::Node::PREDICATE || - first.node_type == plansys2_msgs::msg::Node::FUNCTION) - { + if ( + first.node_type == plansys2_msgs::msg::Node::ACTION || + first.node_type == plansys2_msgs::msg::Node::PREDICATE || + first.node_type == plansys2_msgs::msg::Node::FUNCTION) { if (first.name != second.name) { return false; } @@ -1072,7 +1101,8 @@ bool checkNodeEquality(const plansys2_msgs::msg::Node & first, const plansys2_ms return true; } -bool checkParamEquality(const plansys2_msgs::msg::Param & first, const plansys2_msgs::msg::Param & second) +bool checkParamEquality( + const plansys2_msgs::msg::Param & first, const plansys2_msgs::msg::Param & second) { if (first.name != second.name) { return false; @@ -1087,17 +1117,18 @@ bool empty(const plansys2_msgs::msg::Tree & tree) return true; } - if ((tree.nodes[0].node_type == plansys2_msgs::msg::Node::AND || - tree.nodes[0].node_type == plansys2_msgs::msg::Node::OR || - tree.nodes[0].node_type == plansys2_msgs::msg::Node::NOT || - tree.nodes[0].node_type == plansys2_msgs::msg::Node::EXPRESSION || - tree.nodes[0].node_type == plansys2_msgs::msg::Node::FUNCTION_MODIFIER) && - tree.nodes[0].children.empty()) { + if ( + (tree.nodes[0].node_type == plansys2_msgs::msg::Node::AND || + tree.nodes[0].node_type == plansys2_msgs::msg::Node::OR || + tree.nodes[0].node_type == plansys2_msgs::msg::Node::NOT || + tree.nodes[0].node_type == plansys2_msgs::msg::Node::EXPRESSION || + tree.nodes[0].node_type == plansys2_msgs::msg::Node::FUNCTION_MODIFIER) && + tree.nodes[0].children.empty()) { return true; } return false; } -} // namespace tree } // namespace pddl +} // namespace parser diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/When.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/When.cpp index 488eb192..64b48d35 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/When.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/When.cpp @@ -1,50 +1,60 @@ #include "plansys2_pddl_parser/Domain.h" -namespace parser { namespace pddl { - -void When::PDDLPrint( std::ostream & s, unsigned indent, const TokenStruct< std::string > & ts, const Domain & d ) const { - tabindent( s, indent ); - s << "( when\n"; - if ( pars ) pars->PDDLPrint( s, indent + 1, ts, d ); - else { - tabindent( s, indent + 1 ); - s << "()"; - } - s << "\n"; - if ( cond ) cond->PDDLPrint( s, indent + 1, ts, d ); - else { - tabindent( s, indent + 1 ); - s << "()"; - } - s << "\n"; - tabindent( s, indent ); - s << ")"; +namespace parser +{ +namespace pddl +{ +void When::PDDLPrint( + std::ostream & s, unsigned indent, const TokenStruct & ts, const Domain & d) const +{ + tabindent(s, indent); + s << "( when\n"; + if (pars) + pars->PDDLPrint(s, indent + 1, ts, d); + else { + tabindent(s, indent + 1); + s << "()"; + } + s << "\n"; + if (cond) + cond->PDDLPrint(s, indent + 1, ts, d); + else { + tabindent(s, indent + 1); + s << "()"; + } + s << "\n"; + tabindent(s, indent); + s << ")"; } -plansys2_msgs::msg::Node::SharedPtr When::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace ) const { - throw UnsupportedConstruct("When"); +plansys2_msgs::msg::Node::SharedPtr When::getTree( + plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector & replace) const +{ + throw UnsupportedConstruct("When"); } -void When::parse( Stringreader & f, TokenStruct< std::string > & ts, Domain & d ) { - f.next(); - f.assert_token( "(" ); - if ( f.getChar() != ')' ) { - pars = d.createCondition( f ); - pars->parse( f, ts, d ); - } - else ++f.c; - - f.next(); - f.assert_token( "(" ); - if ( f.getChar() != ')' ) { - cond = d.createCondition( f ); - cond->parse( f, ts, d ); - } - else ++f.c; - - f.next(); - f.assert_token( ")" ); +void When::parse(Stringreader & f, TokenStruct & ts, Domain & d) +{ + f.next(); + f.assert_token("("); + if (f.getChar() != ')') { + pars = d.createCondition(f); + pars->parse(f, ts, d); + } else + ++f.c; + + f.next(); + f.assert_token("("); + if (f.getChar() != ')') { + cond = d.createCondition(f); + cond->parse(f, ts, d); + } else + ++f.c; + + f.next(); + f.assert_token(")"); } -} } // namespaces +} // namespace pddl +} // namespace parser diff --git a/plansys2_pddl_parser/test/pddl/dom1.pddl b/plansys2_pddl_parser/test/pddl/dom1.pddl index 16f14dbe..ad3ade80 100644 --- a/plansys2_pddl_parser/test/pddl/dom1.pddl +++ b/plansys2_pddl_parser/test/pddl/dom1.pddl @@ -58,4 +58,4 @@ ) ) -) \ No newline at end of file +) diff --git a/plansys2_pddl_parser/test/pddl/prob1.pddl b/plansys2_pddl_parser/test/pddl/prob1.pddl index 5ca3bcf9..0eb4d857 100644 --- a/plansys2_pddl_parser/test/pddl/prob1.pddl +++ b/plansys2_pddl_parser/test/pddl/prob1.pddl @@ -32,4 +32,4 @@ )) -) \ No newline at end of file +) diff --git a/plansys2_pddl_parser/test/pddl_parser_test.cpp b/plansys2_pddl_parser/test/pddl_parser_test.cpp index eb226527..7088ad12 100644 --- a/plansys2_pddl_parser/test/pddl_parser_test.cpp +++ b/plansys2_pddl_parser/test/pddl_parser_test.cpp @@ -12,55 +12,49 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include +#include #include +#include #include #include -#include -#include -#include -#include "rclcpp/rclcpp.hpp" #include "ament_index_cpp/get_package_share_directory.hpp" -#include "plansys2_pddl_parser/Instance.h" #include "gtest/gtest.h" +#include "plansys2_pddl_parser/Instance.h" +#include "rclcpp/rclcpp.hpp" using namespace std::chrono_literals; class PDDLParserTestCase : public ::testing::Test { protected: - static void SetUpTestCase() - { - rclcpp::init(0, nullptr); - } + static void SetUpTestCase() { rclcpp::init(0, nullptr); } }; TEST(PDDLParserTestCase, pddl_parser) { - std::string pkgpath = ament_index_cpp::get_package_share_directory("plansys2_pddl_parser"); std::string domain_file = pkgpath + "/pddl/dom1.pddl"; std::string instance_file = pkgpath + "/pddl/prob1.pddl"; std::ifstream domain_ifs(domain_file); ASSERT_TRUE(domain_ifs.good()); - std::string domain_str(( - std::istreambuf_iterator(domain_ifs)), - std::istreambuf_iterator()); + std::string domain_str( + (std::istreambuf_iterator(domain_ifs)), std::istreambuf_iterator()); ASSERT_NE(domain_str, ""); std::ifstream instance_ifs(instance_file); ASSERT_TRUE(instance_ifs.good()); - std::string instance_str(( - std::istreambuf_iterator(instance_ifs)), - std::istreambuf_iterator()); + std::string instance_str( + (std::istreambuf_iterator(instance_ifs)), std::istreambuf_iterator()); ASSERT_NE(instance_str, ""); // Read domain and instance bool okparse = false; bool okprint = false; try { - parser::pddl::Domain domain( domain_str ); - parser::pddl::Instance instance( domain, instance_str ); + parser::pddl::Domain domain(domain_str); + parser::pddl::Instance instance(domain, instance_str); okparse = true; try { std::cout << domain << std::endl; diff --git a/plansys2_planner/CMakeLists.txt b/plansys2_planner/CMakeLists.txt index e9c5506f..7919ecc6 100644 --- a/plansys2_planner/CMakeLists.txt +++ b/plansys2_planner/CMakeLists.txt @@ -69,6 +69,7 @@ install(TARGETS if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) + set(ament_cmake_uncrustify_FOUND TRUE) ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest REQUIRED) diff --git a/plansys2_planner/include/plansys2_planner/PlannerClient.hpp b/plansys2_planner/include/plansys2_planner/PlannerClient.hpp index f695a902..5785346f 100644 --- a/plansys2_planner/include/plansys2_planner/PlannerClient.hpp +++ b/plansys2_planner/include/plansys2_planner/PlannerClient.hpp @@ -19,15 +19,12 @@ #include #include -#include "plansys2_planner/PlannerInterface.hpp" - #include "plansys2_msgs/srv/get_plan.hpp" - +#include "plansys2_planner/PlannerInterface.hpp" #include "rclcpp/rclcpp.hpp" namespace plansys2 { - class PlannerClient : public PlannerInterface { public: @@ -38,8 +35,7 @@ class PlannerClient : public PlannerInterface const std::string & node_namespace = ""); private: - rclcpp::Client::SharedPtr - get_plan_client_; + rclcpp::Client::SharedPtr get_plan_client_; rclcpp::Node::SharedPtr node_; }; diff --git a/plansys2_planner/include/plansys2_planner/PlannerInterface.hpp b/plansys2_planner/include/plansys2_planner/PlannerInterface.hpp index a47d6354..49790a7b 100644 --- a/plansys2_planner/include/plansys2_planner/PlannerInterface.hpp +++ b/plansys2_planner/include/plansys2_planner/PlannerInterface.hpp @@ -22,7 +22,6 @@ namespace plansys2 { - class PlannerInterface { public: diff --git a/plansys2_planner/include/plansys2_planner/PlannerNode.hpp b/plansys2_planner/include/plansys2_planner/PlannerNode.hpp index a87326ec..a6dee305 100644 --- a/plansys2_planner/include/plansys2_planner/PlannerNode.hpp +++ b/plansys2_planner/include/plansys2_planner/PlannerNode.hpp @@ -16,36 +16,30 @@ #define PLANSYS2_PLANNER__PLANNERNODE_HPP_ #include -#include #include +#include #include -#include "plansys2_domain_expert/DomainExpertClient.hpp" -#include "plansys2_problem_expert/ProblemExpertClient.hpp" - -#include "plansys2_core/PlanSolverBase.hpp" - -#include "std_msgs/msg/empty.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "lifecycle_msgs/msg/transition.hpp" +#include "plansys2_core/PlanSolverBase.hpp" +#include "plansys2_domain_expert/DomainExpertClient.hpp" #include "plansys2_msgs/srv/get_plan.hpp" - +#include "plansys2_problem_expert/ProblemExpertClient.hpp" +#include "pluginlib/class_list_macros.hpp" +#include "pluginlib/class_loader.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" - -#include "pluginlib/class_loader.hpp" -#include "pluginlib/class_list_macros.hpp" +#include "std_msgs/msg/empty.hpp" namespace plansys2 { - class PlannerNode : public rclcpp_lifecycle::LifecycleNode { public: PlannerNode(); - using CallbackReturnT = - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + using CallbackReturnT = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; using SolverMap = std::unordered_map; @@ -69,27 +63,23 @@ class PlannerNode : public rclcpp_lifecycle::LifecycleNode std::vector solver_ids_; std::vector solver_types_; - rclcpp::Service::SharedPtr - get_plan_service_; + rclcpp::Service::SharedPtr get_plan_service_; }; -template +template void declare_parameter_if_not_declared( - NodeT node, - const std::string & param_name, + NodeT node, const std::string & param_name, const rclcpp::ParameterValue & default_value = rclcpp::ParameterValue(), const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor = - rcl_interfaces::msg::ParameterDescriptor()) + rcl_interfaces::msg::ParameterDescriptor()) { if (!node->has_parameter(param_name)) { node->declare_parameter(param_name, default_value, parameter_descriptor); } } -template -std::string get_plugin_type_param( - NodeT node, - const std::string & plugin_name) +template +std::string get_plugin_type_param(NodeT node, const std::string & plugin_name) { declare_parameter_if_not_declared(node, plugin_name + ".plugin", rclcpp::ParameterValue("")); std::string plugin_type; diff --git a/plansys2_planner/launch/planner_launch.py b/plansys2_planner/launch/planner_launch.py index 1af046e0..a8101f2f 100644 --- a/plansys2_planner/launch/planner_launch.py +++ b/plansys2_planner/launch/planner_launch.py @@ -1,3 +1,4 @@ +# -*- coding: utf-8 -*- # Copyright 2019 Intelligent Robotics Lab # # Licensed under the Apache License, Version 2.0 (the "License"); @@ -19,22 +20,22 @@ def generate_launch_description(): - namespace = LaunchConfiguration('namespace') - params_file = LaunchConfiguration('params_file') + namespace = LaunchConfiguration("namespace") + params_file = LaunchConfiguration("params_file") declare_namespace_cmd = DeclareLaunchArgument( - 'namespace', - default_value='', - description='Namespace') + "namespace", default_value="", description="Namespace" + ) # Specify the actions planner_cmd = Node( - package='plansys2_planner', - executable='planner_node', - name='planner', + package="plansys2_planner", + executable="planner_node", + name="planner", namespace=namespace, - output='screen', - parameters=[params_file]) + output="screen", + parameters=[params_file], + ) # Create the launch description and populate ld = LaunchDescription() diff --git a/plansys2_planner/package.xml b/plansys2_planner/package.xml index 95b599e8..38d36813 100644 --- a/plansys2_planner/package.xml +++ b/plansys2_planner/package.xml @@ -3,11 +3,11 @@ plansys2_planner 2.0.8 - + This package contains the PDDL-based Planner module for the ROS2 Planning System Francisco Martin Rico - + Apache License, Version 2.0 ament_cmake diff --git a/plansys2_planner/src/plansys2_planner/PlannerClient.cpp b/plansys2_planner/src/plansys2_planner/PlannerClient.cpp index ad39e97c..1a0387d9 100644 --- a/plansys2_planner/src/plansys2_planner/PlannerClient.cpp +++ b/plansys2_planner/src/plansys2_planner/PlannerClient.cpp @@ -14,15 +14,14 @@ #include "plansys2_planner/PlannerClient.hpp" -#include #include +#include +#include #include #include -#include namespace plansys2 { - PlannerClient::PlannerClient() { node_ = rclcpp::Node::make_shared("planner_client"); @@ -30,19 +29,16 @@ PlannerClient::PlannerClient() get_plan_client_ = node_->create_client("planner/get_plan"); } -std::optional -PlannerClient::getPlan( - const std::string & domain, const std::string & problem, - const std::string & node_namespace) +std::optional PlannerClient::getPlan( + const std::string & domain, const std::string & problem, const std::string & node_namespace) { while (!get_plan_client_->wait_for_service(std::chrono::seconds(30))) { if (!rclcpp::ok()) { return {}; } RCLCPP_ERROR_STREAM( - node_->get_logger(), - get_plan_client_->get_service_name() << - " service client: waiting for service to appear..."); + node_->get_logger(), get_plan_client_->get_service_name() << " service client: waiting " + "for service to appear..."); } auto request = std::make_shared(); @@ -51,9 +47,9 @@ PlannerClient::getPlan( auto future_result = get_plan_client_->async_send_request(request); - if (rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(15)) != - rclcpp::FutureReturnCode::SUCCESS) - { + if ( + rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(15)) != + rclcpp::FutureReturnCode::SUCCESS) { return {}; } @@ -63,9 +59,7 @@ PlannerClient::getPlan( return result.plan; } else { RCLCPP_ERROR_STREAM( - node_->get_logger(), - get_plan_client_->get_service_name() << ": " << - result.error_info); + node_->get_logger(), get_plan_client_->get_service_name() << ": " << result.error_info); return {}; } } diff --git a/plansys2_planner/src/plansys2_planner/PlannerNode.cpp b/plansys2_planner/src/plansys2_planner/PlannerNode.cpp index 782e088b..019b53e4 100644 --- a/plansys2_planner/src/plansys2_planner/PlannerNode.cpp +++ b/plansys2_planner/src/plansys2_planner/PlannerNode.cpp @@ -12,19 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include -#include - #include "plansys2_planner/PlannerNode.hpp" -#include "plansys2_popf_plan_solver/popf_plan_solver.hpp" + +#include +#include +#include +#include #include "lifecycle_msgs/msg/state.hpp" +#include "plansys2_popf_plan_solver/popf_plan_solver.hpp" namespace plansys2 { - PlannerNode::PlannerNode() : rclcpp_lifecycle::LifecycleNode("planner"), lp_loader_("plansys2_core", "plansys2::PlanSolverBase"), @@ -32,21 +31,16 @@ PlannerNode::PlannerNode() default_types_{} { get_plan_service_ = create_service( - "planner/get_plan", - std::bind( - &PlannerNode::get_plan_service_callback, - this, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3)); + "planner/get_plan", std::bind( + &PlannerNode::get_plan_service_callback, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3)); declare_parameter("plan_solver_plugins", default_ids_); } +using CallbackReturnT = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; -using CallbackReturnT = - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - -CallbackReturnT -PlannerNode::on_configure(const rclcpp_lifecycle::State & state) +CallbackReturnT PlannerNode::on_configure(const rclcpp_lifecycle::State & state) { auto node = shared_from_this(); @@ -58,8 +52,7 @@ PlannerNode::on_configure(const rclcpp_lifecycle::State & state) if (solver_ids_ == default_ids_) { for (size_t i = 0; i < default_ids_.size(); ++i) { plansys2::declare_parameter_if_not_declared( - node, default_ids_[i] + ".plugin", - rclcpp::ParameterValue(default_types_[i])); + node, default_ids_[i] + ".plugin", rclcpp::ParameterValue(default_types_[i])); } } solver_types_.resize(solver_ids_.size()); @@ -67,14 +60,13 @@ PlannerNode::on_configure(const rclcpp_lifecycle::State & state) for (size_t i = 0; i != solver_types_.size(); i++) { try { solver_types_[i] = plansys2::get_plugin_type_param(node, solver_ids_[i]); - plansys2::PlanSolverBase::Ptr solver = - lp_loader_.createUniqueInstance(solver_types_[i]); + plansys2::PlanSolverBase::Ptr solver = lp_loader_.createUniqueInstance(solver_types_[i]); solver->configure(node, solver_ids_[i]); RCLCPP_INFO( - get_logger(), "Created solver : %s of type %s", - solver_ids_[i].c_str(), solver_types_[i].c_str()); + get_logger(), "Created solver : %s of type %s", solver_ids_[i].c_str(), + solver_types_[i].c_str()); solvers_.insert({solver_ids_[i], solver}); } catch (const pluginlib::PluginlibException & ex) { RCLCPP_FATAL(get_logger(), "Failed to create solver. Exception: %s", ex.what()); @@ -86,24 +78,21 @@ PlannerNode::on_configure(const rclcpp_lifecycle::State & state) default_solver->configure(node, "POPF"); solvers_.insert({"POPF", default_solver}); RCLCPP_INFO( - get_logger(), "Created default solver : %s of type %s", - "POPF", "plansys2/POPFPlanSolver"); + get_logger(), "Created default solver : %s of type %s", "POPF", "plansys2/POPFPlanSolver"); } RCLCPP_INFO(get_logger(), "[%s] Configured", get_name()); return CallbackReturnT::SUCCESS; } -CallbackReturnT -PlannerNode::on_activate(const rclcpp_lifecycle::State & state) +CallbackReturnT PlannerNode::on_activate(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "[%s] Activating...", get_name()); RCLCPP_INFO(get_logger(), "[%s] Activated", get_name()); return CallbackReturnT::SUCCESS; } -CallbackReturnT -PlannerNode::on_deactivate(const rclcpp_lifecycle::State & state) +CallbackReturnT PlannerNode::on_deactivate(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "[%s] Deactivating...", get_name()); RCLCPP_INFO(get_logger(), "[%s] Deactivated", get_name()); @@ -111,8 +100,7 @@ PlannerNode::on_deactivate(const rclcpp_lifecycle::State & state) return CallbackReturnT::SUCCESS; } -CallbackReturnT -PlannerNode::on_cleanup(const rclcpp_lifecycle::State & state) +CallbackReturnT PlannerNode::on_cleanup(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "[%s] Cleaning up...", get_name()); RCLCPP_INFO(get_logger(), "[%s] Cleaned up", get_name()); @@ -120,8 +108,7 @@ PlannerNode::on_cleanup(const rclcpp_lifecycle::State & state) return CallbackReturnT::SUCCESS; } -CallbackReturnT -PlannerNode::on_shutdown(const rclcpp_lifecycle::State & state) +CallbackReturnT PlannerNode::on_shutdown(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "[%s] Shutting down...", get_name()); RCLCPP_INFO(get_logger(), "[%s] Shutted down", get_name()); @@ -129,23 +116,19 @@ PlannerNode::on_shutdown(const rclcpp_lifecycle::State & state) return CallbackReturnT::SUCCESS; } -CallbackReturnT -PlannerNode::on_error(const rclcpp_lifecycle::State & state) +CallbackReturnT PlannerNode::on_error(const rclcpp_lifecycle::State & state) { RCLCPP_ERROR(get_logger(), "[%s] Error transition", get_name()); return CallbackReturnT::SUCCESS; } - -void -PlannerNode::get_plan_service_callback( +void PlannerNode::get_plan_service_callback( const std::shared_ptr request_header, const std::shared_ptr request, const std::shared_ptr response) { - auto plan = solvers_.begin()->second->getPlan( - request->domain, request->problem, get_namespace()); + auto plan = solvers_.begin()->second->getPlan(request->domain, request->problem, get_namespace()); if (plan) { response->success = true; diff --git a/plansys2_planner/test/CMakeLists.txt b/plansys2_planner/test/CMakeLists.txt index 0d63f3ea..0b4105f0 100644 --- a/plansys2_planner/test/CMakeLists.txt +++ b/plansys2_planner/test/CMakeLists.txt @@ -7,4 +7,4 @@ install(DIRECTORY ) add_subdirectory(unit) -#add_subdirectory(integration) \ No newline at end of file +#add_subdirectory(integration) diff --git a/plansys2_planner/test/pddl/domain_simple_constants.pddl b/plansys2_planner/test/pddl/domain_simple_constants.pddl index 2e621029..1ebd3fd1 100644 --- a/plansys2_planner/test/pddl/domain_simple_constants.pddl +++ b/plansys2_planner/test/pddl/domain_simple_constants.pddl @@ -3,7 +3,7 @@ ;; Types ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; (:types -person - object +person - object message - object ; This bracket inside a comment, is here for testing purpose :-) robot - object @@ -81,7 +81,7 @@ jack john - person (:action move_person_john :parameters (?r1 ?r2 - room) - :precondition (and + :precondition (and (person_at john ?r1) ) :effect (and @@ -93,7 +93,7 @@ jack john - person (:action move_person_jack :parameters (?r1 ?r2 - room) - :precondition (and + :precondition (and (person_at jack ?r1) ) :effect (and diff --git a/plansys2_planner/test/pddl/problem_simple_1.pddl b/plansys2_planner/test/pddl/problem_simple_1.pddl index adb76c85..2de36ca2 100644 --- a/plansys2_planner/test/pddl/problem_simple_1.pddl +++ b/plansys2_planner/test/pddl/problem_simple_1.pddl @@ -15,7 +15,7 @@ ;; The goal is to have both packages delivered to their destinations: (:goal (and - (robot_talk leia m1 Jack) + (robot_talk leia m1 Jack) ) ) ) diff --git a/plansys2_planner/test/pddl/problem_simple_2.pddl b/plansys2_planner/test/pddl/problem_simple_2.pddl index dfc51ee1..05fb33a7 100644 --- a/plansys2_planner/test/pddl/problem_simple_2.pddl +++ b/plansys2_planner/test/pddl/problem_simple_2.pddl @@ -14,7 +14,7 @@ ;; The goal is to have both packages delivered to their destinations: (:goal (and - (robot_talk leia m1 Jack) + (robot_talk leia m1 Jack) ) ) ) diff --git a/plansys2_planner/test/pddl/problem_simple_3.pddl b/plansys2_planner/test/pddl/problem_simple_3.pddl index 056ec3e6..21cc29e2 100644 --- a/plansys2_planner/test/pddl/problem_simple_3.pddl +++ b/plansys2_planner/test/pddl/problem_simple_3.pddl @@ -12,7 +12,7 @@ ;; The goal is to have both packages delivered to their destinations: (:goa (and - (robot_talk leia m1 Jack) - + (robot_talk leia m1 Jack) + ) ) diff --git a/plansys2_planner/test/unit/CMakeLists.txt b/plansys2_planner/test/unit/CMakeLists.txt index a1dc2e04..53083fa1 100644 --- a/plansys2_planner/test/unit/CMakeLists.txt +++ b/plansys2_planner/test/unit/CMakeLists.txt @@ -1,3 +1,3 @@ ament_add_gtest(planner_test planner_test.cpp) target_link_libraries(planner_test ${PROJECT_NAME} dl) -target_compile_definitions(planner_test PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") \ No newline at end of file +target_compile_definitions(planner_test PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") diff --git a/plansys2_planner/test/unit/planner_test.cpp b/plansys2_planner/test/unit/planner_test.cpp index 961bb2a9..3b284eb5 100644 --- a/plansys2_planner/test/unit/planner_test.cpp +++ b/plansys2_planner/test/unit/planner_test.cpp @@ -12,30 +12,26 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include +#include +#include #include #include -#include -#include -#include #include "ament_index_cpp/get_package_share_directory.hpp" - #include "gtest/gtest.h" -#include "plansys2_domain_expert/DomainExpertNode.hpp" #include "plansys2_domain_expert/DomainExpertClient.hpp" +#include "plansys2_domain_expert/DomainExpertNode.hpp" #include "plansys2_msgs/msg/param.h" #include "plansys2_pddl_parser/Utils.h" -#include "plansys2_problem_expert/ProblemExpertNode.hpp" -#include "plansys2_problem_expert/ProblemExpertClient.hpp" -#include "plansys2_planner/PlannerNode.hpp" #include "plansys2_planner/PlannerClient.hpp" - -#include "pluginlib/class_loader.hpp" +#include "plansys2_planner/PlannerNode.hpp" +#include "plansys2_problem_expert/ProblemExpertClient.hpp" +#include "plansys2_problem_expert/ProblemExpertNode.hpp" #include "pluginlib/class_list_macros.hpp" - +#include "pluginlib/class_loader.hpp" #include "rclcpp/rclcpp.hpp" - TEST(planner_expert, generate_plan_good) { auto test_node = rclcpp::Node::make_shared("test_node"); @@ -59,9 +55,10 @@ TEST(planner_expert, generate_plan_good) bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} - }); - + while (!finish) { + exe.spin_some(); + } + }); domain_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); problem_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); @@ -97,8 +94,7 @@ TEST(planner_expert, generate_plan_good) ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("corridor", "room"))); std::vector predicates = { - "(robot_at leia kitchen)", - "(person_at francisco bedroom)"}; + "(robot_at leia kitchen)", "(person_at francisco bedroom)"}; for (const auto & pred : predicates) { ASSERT_TRUE(problem_client->addPredicate(plansys2::Predicate(pred))); @@ -114,7 +110,6 @@ TEST(planner_expert, generate_plan_good) t.join(); } - TEST(planner_expert, generate_plan_with_domain_constants) { auto test_node = rclcpp::Node::make_shared("test_node"); @@ -138,9 +133,10 @@ TEST(planner_expert, generate_plan_with_domain_constants) bool finish = false; std::thread t([&]() { - while (!finish) {exe.spin_some();} - }); - + while (!finish) { + exe.spin_some(); + } + }); domain_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); problem_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); @@ -168,9 +164,8 @@ TEST(planner_expert, generate_plan_with_domain_constants) } std::ifstream problem_1_ifs(pkgpath + "/pddl/problem_simple_constants_1.pddl"); - std::string problem_1_str(( - std::istreambuf_iterator(problem_1_ifs)), - std::istreambuf_iterator()); + std::string problem_1_str( + (std::istreambuf_iterator(problem_1_ifs)), std::istreambuf_iterator()); ASSERT_TRUE(problem_client->addProblem(problem_1_str)); ASSERT_EQ(problem_client->getProblem(), problem_1_str); @@ -178,13 +173,11 @@ TEST(planner_expert, generate_plan_with_domain_constants) auto plan = planner_client->getPlan(domain_client->getDomain(), problem_client->getProblem()); ASSERT_TRUE(plan); - problem_client->clearKnowledge(); std::ifstream problem_2_ifs(pkgpath + "/pddl/problem_simple_constants_2.pddl"); - std::string problem_2_str(( - std::istreambuf_iterator(problem_2_ifs)), - std::istreambuf_iterator()); + std::string problem_2_str( + (std::istreambuf_iterator(problem_2_ifs)), std::istreambuf_iterator()); ASSERT_TRUE(problem_client->addProblem(problem_2_str)); ASSERT_NE(problem_client->getProblem(), problem_2_str); @@ -193,7 +186,6 @@ TEST(planner_expert, generate_plan_with_domain_constants) plan = planner_client->getPlan(domain_client->getDomain(), problem_client->getProblem()); ASSERT_TRUE(plan); - finish = true; t.join(); } diff --git a/plansys2_popf_plan_solver/CMakeLists.txt b/plansys2_popf_plan_solver/CMakeLists.txt index 9868495f..8f809199 100644 --- a/plansys2_popf_plan_solver/CMakeLists.txt +++ b/plansys2_popf_plan_solver/CMakeLists.txt @@ -47,6 +47,7 @@ install(TARGETS if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) + set(ament_cmake_uncrustify_FOUND TRUE) ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest REQUIRED) @@ -57,4 +58,4 @@ ament_export_include_directories(include) ament_export_libraries(${PROJECT_NAME}) ament_export_dependencies(${dependencies}) -ament_package() \ No newline at end of file +ament_package() diff --git a/plansys2_popf_plan_solver/include/plansys2_popf_plan_solver/popf_plan_solver.hpp b/plansys2_popf_plan_solver/include/plansys2_popf_plan_solver/popf_plan_solver.hpp index 2765cbd7..3e8694dd 100644 --- a/plansys2_popf_plan_solver/include/plansys2_popf_plan_solver/popf_plan_solver.hpp +++ b/plansys2_popf_plan_solver/include/plansys2_popf_plan_solver/popf_plan_solver.hpp @@ -15,15 +15,14 @@ #ifndef PLANSYS2_POPF_PLAN_SOLVER__POPF_PLAN_SOLVER_HPP_ #define PLANSYS2_POPF_PLAN_SOLVER__POPF_PLAN_SOLVER_HPP_ -#include #include +#include #include #include "plansys2_core/PlanSolverBase.hpp" namespace plansys2 { - class POPFPlanSolver : public PlanSolverBase { private: @@ -39,9 +38,7 @@ class POPFPlanSolver : public PlanSolverBase const std::string & domain, const std::string & problem, const std::string & node_namespace = ""); - bool is_valid_domain( - const std::string & domain, - const std::string & node_namespace = ""); + bool is_valid_domain(const std::string & domain, const std::string & node_namespace = ""); }; } // namespace plansys2 diff --git a/plansys2_popf_plan_solver/package.xml b/plansys2_popf_plan_solver/package.xml index a52c95b1..36e0a425 100644 --- a/plansys2_popf_plan_solver/package.xml +++ b/plansys2_popf_plan_solver/package.xml @@ -3,11 +3,11 @@ plansys2_popf_plan_solver 2.0.8 - + This package contains the PDDL-based Planner module for the ROS2 Planning System Francisco Martin Rico - + Apache License, Version 2.0 ament_cmake @@ -16,7 +16,7 @@ ament_index_cpp plansys2_core pluginlib - + popf ament_lint_common diff --git a/plansys2_popf_plan_solver/src/plansys2_popf_plan_solver/popf_plan_solver.cpp b/plansys2_popf_plan_solver/src/plansys2_popf_plan_solver/popf_plan_solver.cpp index f7ca968e..19a61332 100644 --- a/plansys2_popf_plan_solver/src/plansys2_popf_plan_solver/popf_plan_solver.cpp +++ b/plansys2_popf_plan_solver/src/plansys2_popf_plan_solver/popf_plan_solver.cpp @@ -12,39 +12,34 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "plansys2_popf_plan_solver/popf_plan_solver.hpp" + #include #include -#include -#include -#include #include #include +#include #include +#include +#include #include "plansys2_msgs/msg/plan_item.hpp" -#include "plansys2_popf_plan_solver/popf_plan_solver.hpp" namespace plansys2 { - -POPFPlanSolver::POPFPlanSolver() -{ -} +POPFPlanSolver::POPFPlanSolver() {} void POPFPlanSolver::configure( - rclcpp_lifecycle::LifecycleNode::SharedPtr & lc_node, - const std::string & plugin_name) + rclcpp_lifecycle::LifecycleNode::SharedPtr & lc_node, const std::string & plugin_name) { parameter_name_ = plugin_name + ".arguments"; lc_node_ = lc_node; lc_node_->declare_parameter(parameter_name_, ""); } -std::optional -POPFPlanSolver::getPlan( - const std::string & domain, const std::string & problem, - const std::string & node_namespace) +std::optional POPFPlanSolver::getPlan( + const std::string & domain, const std::string & problem, const std::string & node_namespace) { if (system(nullptr) == 0) { return {}; @@ -52,7 +47,7 @@ POPFPlanSolver::getPlan( if (node_namespace != "") { std::filesystem::path tp = std::filesystem::temp_directory_path(); - for (auto p : std::filesystem::path(node_namespace) ) { + for (auto p : std::filesystem::path(node_namespace)) { if (p != std::filesystem::current_path().root_directory()) { tp /= p; } @@ -69,11 +64,11 @@ POPFPlanSolver::getPlan( problem_out << problem; problem_out.close(); - int status = system( - ("ros2 run popf popf " + - lc_node_->get_parameter(parameter_name_).value_to_string() + - " /tmp/" + node_namespace + "/domain.pddl /tmp/" + node_namespace + - "/problem.pddl > /tmp/" + node_namespace + "/plan").c_str()); + int status = + system(("ros2 run popf popf " + lc_node_->get_parameter(parameter_name_).value_to_string() + + " /tmp/" + node_namespace + "/domain.pddl /tmp/" + node_namespace + + "/problem.pddl > /tmp/" + node_namespace + "/plan") + .c_str()); if (status == -1) { return {}; @@ -117,10 +112,7 @@ POPFPlanSolver::getPlan( return ret; } -bool -POPFPlanSolver::is_valid_domain( - const std::string & domain, - const std::string & node_namespace) +bool POPFPlanSolver::is_valid_domain(const std::string & domain, const std::string & node_namespace) { if (system(nullptr) == 0) { return false; @@ -128,7 +120,7 @@ POPFPlanSolver::is_valid_domain( std::filesystem::path temp_dir = std::filesystem::temp_directory_path(); if (node_namespace != "") { - for (auto p : std::filesystem::path(node_namespace) ) { + for (auto p : std::filesystem::path(node_namespace)) { if (p != std::filesystem::current_path().root_directory()) { temp_dir /= p; } @@ -144,9 +136,10 @@ POPFPlanSolver::is_valid_domain( problem_out << "(define (problem void) (:domain plansys2))"; problem_out.close(); - int status = system( - ("ros2 run popf popf " + temp_dir.string() + "/check_domain.pddl " + temp_dir.string() + - "/check_problem.pddl > " + temp_dir.string() + "/check.out").c_str()); + int status = + system(("ros2 run popf popf " + temp_dir.string() + "/check_domain.pddl " + temp_dir.string() + + "/check_problem.pddl > " + temp_dir.string() + "/check.out") + .c_str()); if (status == -1) { return false; diff --git a/plansys2_popf_plan_solver/test/CMakeLists.txt b/plansys2_popf_plan_solver/test/CMakeLists.txt index 0d63f3ea..0b4105f0 100644 --- a/plansys2_popf_plan_solver/test/CMakeLists.txt +++ b/plansys2_popf_plan_solver/test/CMakeLists.txt @@ -7,4 +7,4 @@ install(DIRECTORY ) add_subdirectory(unit) -#add_subdirectory(integration) \ No newline at end of file +#add_subdirectory(integration) diff --git a/plansys2_popf_plan_solver/test/pddl/problem_simple_1.pddl b/plansys2_popf_plan_solver/test/pddl/problem_simple_1.pddl index adb76c85..2de36ca2 100644 --- a/plansys2_popf_plan_solver/test/pddl/problem_simple_1.pddl +++ b/plansys2_popf_plan_solver/test/pddl/problem_simple_1.pddl @@ -15,7 +15,7 @@ ;; The goal is to have both packages delivered to their destinations: (:goal (and - (robot_talk leia m1 Jack) + (robot_talk leia m1 Jack) ) ) ) diff --git a/plansys2_popf_plan_solver/test/pddl/problem_simple_2.pddl b/plansys2_popf_plan_solver/test/pddl/problem_simple_2.pddl index dfc51ee1..05fb33a7 100644 --- a/plansys2_popf_plan_solver/test/pddl/problem_simple_2.pddl +++ b/plansys2_popf_plan_solver/test/pddl/problem_simple_2.pddl @@ -14,7 +14,7 @@ ;; The goal is to have both packages delivered to their destinations: (:goal (and - (robot_talk leia m1 Jack) + (robot_talk leia m1 Jack) ) ) ) diff --git a/plansys2_popf_plan_solver/test/pddl/problem_simple_3.pddl b/plansys2_popf_plan_solver/test/pddl/problem_simple_3.pddl index 056ec3e6..21cc29e2 100644 --- a/plansys2_popf_plan_solver/test/pddl/problem_simple_3.pddl +++ b/plansys2_popf_plan_solver/test/pddl/problem_simple_3.pddl @@ -12,7 +12,7 @@ ;; The goal is to have both packages delivered to their destinations: (:goa (and - (robot_talk leia m1 Jack) - + (robot_talk leia m1 Jack) + ) ) diff --git a/plansys2_popf_plan_solver/test/unit/CMakeLists.txt b/plansys2_popf_plan_solver/test/unit/CMakeLists.txt index 6f23dd16..1f1df3df 100644 --- a/plansys2_popf_plan_solver/test/unit/CMakeLists.txt +++ b/plansys2_popf_plan_solver/test/unit/CMakeLists.txt @@ -1,3 +1,3 @@ ament_add_gtest(popf_test popf_test.cpp) target_link_libraries(popf_test ${PROJECT_NAME} dl) -target_compile_definitions(popf_test PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") \ No newline at end of file +target_compile_definitions(popf_test PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") diff --git a/plansys2_popf_plan_solver/test/unit/popf_test.cpp b/plansys2_popf_plan_solver/test/unit/popf_test.cpp index a011a1d4..83fd74fc 100644 --- a/plansys2_popf_plan_solver/test/unit/popf_test.cpp +++ b/plansys2_popf_plan_solver/test/unit/popf_test.cpp @@ -12,33 +12,29 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include +#include +#include #include #include -#include -#include -#include #include "ament_index_cpp/get_package_share_directory.hpp" - #include "gtest/gtest.h" +#include "plansys2_core/PlanSolverBase.hpp" #include "plansys2_popf_plan_solver/popf_plan_solver.hpp" - -#include "pluginlib/class_loader.hpp" #include "pluginlib/class_list_macros.hpp" -#include "plansys2_core/PlanSolverBase.hpp" +#include "pluginlib/class_loader.hpp" void test_plan_generation(const std::string & argument = "") { std::string pkgpath = ament_index_cpp::get_package_share_directory("plansys2_popf_plan_solver"); std::ifstream domain_ifs(pkgpath + "/pddl/domain_simple.pddl"); - std::string domain_str(( - std::istreambuf_iterator(domain_ifs)), - std::istreambuf_iterator()); + std::string domain_str( + (std::istreambuf_iterator(domain_ifs)), std::istreambuf_iterator()); std::ifstream problem_ifs(pkgpath + "/pddl/problem_simple_1.pddl"); - std::string problem_str(( - std::istreambuf_iterator(problem_ifs)), - std::istreambuf_iterator()); + std::string problem_str( + (std::istreambuf_iterator(problem_ifs)), std::istreambuf_iterator()); auto node = rclcpp_lifecycle::LifecycleNode::make_shared("test_node"); auto planner = std::make_shared(); @@ -54,15 +50,9 @@ void test_plan_generation(const std::string & argument = "") ASSERT_EQ(plan.value().items[2].action, "(talk leia jack jack m1)"); } -TEST(popf_plan_solver, generate_plan_good) -{ - test_plan_generation(); -} +TEST(popf_plan_solver, generate_plan_good) { test_plan_generation(); } -TEST(popf_plan_solver, generate_plan_good_with_argument) -{ - test_plan_generation("-e"); -} +TEST(popf_plan_solver, generate_plan_good_with_argument) { test_plan_generation("-e"); } TEST(popf_plan_solver, load_popf_plugin) { @@ -82,9 +72,8 @@ TEST(popf_plan_solver, check_1_ok_domain) { std::string pkgpath = ament_index_cpp::get_package_share_directory("plansys2_popf_plan_solver"); std::ifstream domain_ifs(pkgpath + "/pddl/domain_1_ok.pddl"); - std::string domain_str(( - std::istreambuf_iterator(domain_ifs)), - std::istreambuf_iterator()); + std::string domain_str( + (std::istreambuf_iterator(domain_ifs)), std::istreambuf_iterator()); auto node = rclcpp_lifecycle::LifecycleNode::make_shared("test_node"); auto planner = std::make_shared(); @@ -95,14 +84,12 @@ TEST(popf_plan_solver, check_1_ok_domain) ASSERT_TRUE(result); } - TEST(popf_plan_solver, check_2_error_domain) { std::string pkgpath = ament_index_cpp::get_package_share_directory("plansys2_popf_plan_solver"); std::ifstream domain_ifs(pkgpath + "/pddl/domain_2_error.pddl"); - std::string domain_str(( - std::istreambuf_iterator(domain_ifs)), - std::istreambuf_iterator()); + std::string domain_str( + (std::istreambuf_iterator(domain_ifs)), std::istreambuf_iterator()); auto node = rclcpp_lifecycle::LifecycleNode::make_shared("test_node"); auto planner = std::make_shared(); @@ -113,19 +100,16 @@ TEST(popf_plan_solver, check_2_error_domain) ASSERT_FALSE(result); } - TEST(popf_plan_solver, generate_plan_unsolvable) { std::string pkgpath = ament_index_cpp::get_package_share_directory("plansys2_popf_plan_solver"); std::ifstream domain_ifs(pkgpath + "/pddl/domain_simple.pddl"); - std::string domain_str(( - std::istreambuf_iterator(domain_ifs)), - std::istreambuf_iterator()); + std::string domain_str( + (std::istreambuf_iterator(domain_ifs)), std::istreambuf_iterator()); std::ifstream problem_ifs(pkgpath + "/pddl/problem_simple_2.pddl"); - std::string problem_str(( - std::istreambuf_iterator(problem_ifs)), - std::istreambuf_iterator()); + std::string problem_str( + (std::istreambuf_iterator(problem_ifs)), std::istreambuf_iterator()); auto node = rclcpp_lifecycle::LifecycleNode::make_shared("test_node"); auto planner = std::make_shared(); @@ -140,14 +124,12 @@ TEST(popf_plan_solver, generate_plan_error) { std::string pkgpath = ament_index_cpp::get_package_share_directory("plansys2_popf_plan_solver"); std::ifstream domain_ifs(pkgpath + "/pddl/domain_simple.pddl"); - std::string domain_str(( - std::istreambuf_iterator(domain_ifs)), - std::istreambuf_iterator()); + std::string domain_str( + (std::istreambuf_iterator(domain_ifs)), std::istreambuf_iterator()); std::ifstream problem_ifs(pkgpath + "/pddl/problem_simple_3.pddl"); - std::string problem_str(( - std::istreambuf_iterator(problem_ifs)), - std::istreambuf_iterator()); + std::string problem_str( + (std::istreambuf_iterator(problem_ifs)), std::istreambuf_iterator()); auto node = rclcpp_lifecycle::LifecycleNode::make_shared("test_node"); auto planner = std::make_shared(); diff --git a/plansys2_problem_expert/CMakeLists.txt b/plansys2_problem_expert/CMakeLists.txt index 8dc2afde..718e1892 100644 --- a/plansys2_problem_expert/CMakeLists.txt +++ b/plansys2_problem_expert/CMakeLists.txt @@ -63,6 +63,7 @@ install(TARGETS if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) + set(ament_cmake_uncrustify_FOUND TRUE) ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest REQUIRED) diff --git a/plansys2_problem_expert/include/plansys2_problem_expert/ProblemExpert.hpp b/plansys2_problem_expert/include/plansys2_problem_expert/ProblemExpert.hpp index 01917c0e..89c379da 100644 --- a/plansys2_problem_expert/include/plansys2_problem_expert/ProblemExpert.hpp +++ b/plansys2_problem_expert/include/plansys2_problem_expert/ProblemExpert.hpp @@ -15,22 +15,20 @@ #ifndef PLANSYS2_PROBLEM_EXPERT__PROBLEMEXPERT_HPP_ #define PLANSYS2_PROBLEM_EXPERT__PROBLEMEXPERT_HPP_ +#include #include #include #include -#include +#include "plansys2_domain_expert/DomainExpert.hpp" #include "plansys2_msgs/msg/node.hpp" #include "plansys2_msgs/msg/param.hpp" #include "plansys2_msgs/msg/tree.hpp" - #include "plansys2_pddl_parser/Utils.h" #include "plansys2_problem_expert/ProblemExpertInterface.hpp" -#include "plansys2_domain_expert/DomainExpert.hpp" namespace plansys2 { - class ProblemExpert : public ProblemExpertInterface { public: @@ -72,16 +70,13 @@ class ProblemExpert : public ProblemExpertInterface private: bool checkPredicateTreeTypes( - const plansys2_msgs::msg::Tree & tree, - std::shared_ptr & domain_expert_, + const plansys2_msgs::msg::Tree & tree, std::shared_ptr & domain_expert_, uint8_t node_id = 0); void removeInvalidPredicates( - std::vector & predicates, - const plansys2::Instance & instance); + std::vector & predicates, const plansys2::Instance & instance); void removeInvalidFunctions( - std::vector & functions, - const plansys2::Instance & instance); + std::vector & functions, const plansys2::Instance & instance); void removeInvalidGoals(const plansys2::Instance & instance); std::vector instances_; diff --git a/plansys2_problem_expert/include/plansys2_problem_expert/ProblemExpertClient.hpp b/plansys2_problem_expert/include/plansys2_problem_expert/ProblemExpertClient.hpp index 9bacb59f..29b21555 100644 --- a/plansys2_problem_expert/include/plansys2_problem_expert/ProblemExpertClient.hpp +++ b/plansys2_problem_expert/include/plansys2_problem_expert/ProblemExpertClient.hpp @@ -19,9 +19,7 @@ #include #include -#include "plansys2_problem_expert/ProblemExpertInterface.hpp" #include "plansys2_core/Types.hpp" - #include "plansys2_msgs/msg/node.hpp" #include "plansys2_msgs/msg/param.hpp" #include "plansys2_msgs/msg/tree.hpp" @@ -29,22 +27,21 @@ #include "plansys2_msgs/srv/add_problem_goal.hpp" #include "plansys2_msgs/srv/affect_node.hpp" #include "plansys2_msgs/srv/affect_param.hpp" +#include "plansys2_msgs/srv/clear_problem_knowledge.hpp" #include "plansys2_msgs/srv/exist_node.hpp" +#include "plansys2_msgs/srv/get_node_details.hpp" #include "plansys2_msgs/srv/get_problem.hpp" #include "plansys2_msgs/srv/get_problem_goal.hpp" #include "plansys2_msgs/srv/get_problem_instance_details.hpp" #include "plansys2_msgs/srv/get_problem_instances.hpp" -#include "plansys2_msgs/srv/get_node_details.hpp" #include "plansys2_msgs/srv/get_states.hpp" #include "plansys2_msgs/srv/is_problem_goal_satisfied.hpp" #include "plansys2_msgs/srv/remove_problem_goal.hpp" -#include "plansys2_msgs/srv/clear_problem_knowledge.hpp" - +#include "plansys2_problem_expert/ProblemExpertInterface.hpp" #include "rclcpp/rclcpp.hpp" namespace plansys2 { - class ProblemExpertClient : public ProblemExpertInterface { public: @@ -78,51 +75,34 @@ class ProblemExpertClient : public ProblemExpertInterface std::string getProblem(); bool addProblem(const std::string & problem_str); - rclcpp::Time getUpdateTime() const {return update_time_;} + rclcpp::Time getUpdateTime() const { return update_time_; } private: - rclcpp::Client::SharedPtr - add_problem_client_; - rclcpp::Client::SharedPtr - add_problem_goal_client_; - rclcpp::Client::SharedPtr - add_problem_instance_client_; - rclcpp::Client::SharedPtr - add_problem_predicate_client_; - rclcpp::Client::SharedPtr - add_problem_function_client_; - rclcpp::Client::SharedPtr - get_problem_goal_client_; + rclcpp::Client::SharedPtr add_problem_client_; + rclcpp::Client::SharedPtr add_problem_goal_client_; + rclcpp::Client::SharedPtr add_problem_instance_client_; + rclcpp::Client::SharedPtr add_problem_predicate_client_; + rclcpp::Client::SharedPtr add_problem_function_client_; + rclcpp::Client::SharedPtr get_problem_goal_client_; rclcpp::Client::SharedPtr get_problem_instance_details_client_; - rclcpp::Client::SharedPtr - get_problem_instances_client_; + rclcpp::Client::SharedPtr get_problem_instances_client_; rclcpp::Client::SharedPtr get_problem_predicate_details_client_; - rclcpp::Client::SharedPtr - get_problem_predicates_client_; + rclcpp::Client::SharedPtr get_problem_predicates_client_; rclcpp::Client::SharedPtr get_problem_function_details_client_; - rclcpp::Client::SharedPtr - get_problem_functions_client_; - rclcpp::Client::SharedPtr - get_problem_client_; - rclcpp::Client::SharedPtr - remove_problem_goal_client_; + rclcpp::Client::SharedPtr get_problem_functions_client_; + rclcpp::Client::SharedPtr get_problem_client_; + rclcpp::Client::SharedPtr remove_problem_goal_client_; rclcpp::Client::SharedPtr clear_problem_knowledge_client_; - rclcpp::Client::SharedPtr - remove_problem_instance_client_; - rclcpp::Client::SharedPtr - remove_problem_predicate_client_; - rclcpp::Client::SharedPtr - remove_problem_function_client_; - rclcpp::Client::SharedPtr - exist_problem_predicate_client_; - rclcpp::Client::SharedPtr - exist_problem_function_client_; - rclcpp::Client::SharedPtr - update_problem_function_client_; + rclcpp::Client::SharedPtr remove_problem_instance_client_; + rclcpp::Client::SharedPtr remove_problem_predicate_client_; + rclcpp::Client::SharedPtr remove_problem_function_client_; + rclcpp::Client::SharedPtr exist_problem_predicate_client_; + rclcpp::Client::SharedPtr exist_problem_function_client_; + rclcpp::Client::SharedPtr update_problem_function_client_; rclcpp::Client::SharedPtr is_problem_goal_satisfied_client_; rclcpp::Node::SharedPtr node_; diff --git a/plansys2_problem_expert/include/plansys2_problem_expert/ProblemExpertInterface.hpp b/plansys2_problem_expert/include/plansys2_problem_expert/ProblemExpertInterface.hpp index e95d6da5..d3a40c50 100644 --- a/plansys2_problem_expert/include/plansys2_problem_expert/ProblemExpertInterface.hpp +++ b/plansys2_problem_expert/include/plansys2_problem_expert/ProblemExpertInterface.hpp @@ -15,18 +15,17 @@ #ifndef PLANSYS2_PROBLEM_EXPERT__PROBLEMEXPERTINTERFACE_HPP_ #define PLANSYS2_PROBLEM_EXPERT__PROBLEMEXPERTINTERFACE_HPP_ +#include #include #include +#include "plansys2_core/Types.hpp" #include "plansys2_msgs/msg/node.hpp" #include "plansys2_msgs/msg/param.hpp" #include "plansys2_msgs/msg/tree.hpp" -#include "plansys2_core/Types.hpp" - namespace plansys2 { - class ProblemExpertInterface { public: diff --git a/plansys2_problem_expert/include/plansys2_problem_expert/ProblemExpertNode.hpp b/plansys2_problem_expert/include/plansys2_problem_expert/ProblemExpertNode.hpp index 7e7a48e4..67cc302e 100644 --- a/plansys2_problem_expert/include/plansys2_problem_expert/ProblemExpertNode.hpp +++ b/plansys2_problem_expert/include/plansys2_problem_expert/ProblemExpertNode.hpp @@ -17,41 +17,37 @@ #include -#include "plansys2_problem_expert/ProblemExpert.hpp" - -#include "std_msgs/msg/string.hpp" -#include "std_msgs/msg/empty.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "lifecycle_msgs/msg/transition.hpp" #include "plansys2_msgs/msg/knowledge.hpp" -#include "plansys2_msgs/srv/affect_node.hpp" -#include "plansys2_msgs/srv/affect_param.hpp" #include "plansys2_msgs/srv/add_problem.hpp" #include "plansys2_msgs/srv/add_problem_goal.hpp" +#include "plansys2_msgs/srv/affect_node.hpp" +#include "plansys2_msgs/srv/affect_param.hpp" +#include "plansys2_msgs/srv/clear_problem_knowledge.hpp" #include "plansys2_msgs/srv/exist_node.hpp" +#include "plansys2_msgs/srv/get_node_details.hpp" #include "plansys2_msgs/srv/get_problem.hpp" #include "plansys2_msgs/srv/get_problem_goal.hpp" #include "plansys2_msgs/srv/get_problem_instance_details.hpp" #include "plansys2_msgs/srv/get_problem_instances.hpp" -#include "plansys2_msgs/srv/get_node_details.hpp" #include "plansys2_msgs/srv/get_states.hpp" #include "plansys2_msgs/srv/is_problem_goal_satisfied.hpp" #include "plansys2_msgs/srv/remove_problem_goal.hpp" -#include "plansys2_msgs/srv/clear_problem_knowledge.hpp" - +#include "plansys2_problem_expert/ProblemExpert.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "std_msgs/msg/empty.hpp" +#include "std_msgs/msg/string.hpp" namespace plansys2 { - class ProblemExpertNode : public rclcpp_lifecycle::LifecycleNode { public: ProblemExpertNode(); - using CallbackReturnT = - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + using CallbackReturnT = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; CallbackReturnT on_configure(const rclcpp_lifecycle::State & state); CallbackReturnT on_activate(const rclcpp_lifecycle::State & state); @@ -175,50 +171,34 @@ class ProblemExpertNode : public rclcpp_lifecycle::LifecycleNode private: std::shared_ptr problem_expert_; - rclcpp::Service::SharedPtr - add_problem_service_; - rclcpp::Service::SharedPtr - add_problem_goal_service_; - rclcpp::Service::SharedPtr - add_problem_instance_service_; - rclcpp::Service::SharedPtr - add_problem_predicate_service_; - rclcpp::Service::SharedPtr - add_problem_function_service_; - rclcpp::Service::SharedPtr - get_problem_goal_service_; + rclcpp::Service::SharedPtr add_problem_service_; + rclcpp::Service::SharedPtr add_problem_goal_service_; + rclcpp::Service::SharedPtr add_problem_instance_service_; + rclcpp::Service::SharedPtr add_problem_predicate_service_; + rclcpp::Service::SharedPtr add_problem_function_service_; + rclcpp::Service::SharedPtr get_problem_goal_service_; rclcpp::Service::SharedPtr get_problem_instance_details_service_; rclcpp::Service::SharedPtr get_problem_instances_service_; rclcpp::Service::SharedPtr get_problem_predicate_details_service_; - rclcpp::Service::SharedPtr - get_problem_predicates_service_; + rclcpp::Service::SharedPtr get_problem_predicates_service_; rclcpp::Service::SharedPtr get_problem_function_details_service_; - rclcpp::Service::SharedPtr - get_problem_functions_service_; - rclcpp::Service::SharedPtr - get_problem_service_; + rclcpp::Service::SharedPtr get_problem_functions_service_; + rclcpp::Service::SharedPtr get_problem_service_; rclcpp::Service::SharedPtr is_problem_goal_satisfied_service_; - rclcpp::Service::SharedPtr - remove_problem_goal_service_; + rclcpp::Service::SharedPtr remove_problem_goal_service_; rclcpp::Service::SharedPtr clear_problem_knowledge_service_; - rclcpp::Service::SharedPtr - remove_problem_instance_service_; - rclcpp::Service::SharedPtr - remove_problem_predicate_service_; - rclcpp::Service::SharedPtr - remove_problem_function_service_; - rclcpp::Service::SharedPtr - exist_problem_predicate_service_; - rclcpp::Service::SharedPtr - exist_problem_function_service_; - rclcpp::Service::SharedPtr - update_problem_function_service_; + rclcpp::Service::SharedPtr remove_problem_instance_service_; + rclcpp::Service::SharedPtr remove_problem_predicate_service_; + rclcpp::Service::SharedPtr remove_problem_function_service_; + rclcpp::Service::SharedPtr exist_problem_predicate_service_; + rclcpp::Service::SharedPtr exist_problem_function_service_; + rclcpp::Service::SharedPtr update_problem_function_service_; rclcpp_lifecycle::LifecyclePublisher::SharedPtr update_pub_; rclcpp_lifecycle::LifecyclePublisher::SharedPtr knowledge_pub_; diff --git a/plansys2_problem_expert/include/plansys2_problem_expert/Utils.hpp b/plansys2_problem_expert/include/plansys2_problem_expert/Utils.hpp index 20b76bd0..dfd822e6 100644 --- a/plansys2_problem_expert/include/plansys2_problem_expert/Utils.hpp +++ b/plansys2_problem_expert/include/plansys2_problem_expert/Utils.hpp @@ -15,21 +15,20 @@ #ifndef PLANSYS2_PROBLEM_EXPERT__UTILS_HPP_ #define PLANSYS2_PROBLEM_EXPERT__UTILS_HPP_ -#include -#include -#include #include -#include +#include #include +#include +#include #include +#include -#include "plansys2_problem_expert/ProblemExpertClient.hpp" #include "plansys2_domain_expert/DomainExpertClient.hpp" #include "plansys2_msgs/msg/tree.hpp" +#include "plansys2_problem_expert/ProblemExpertClient.hpp" namespace plansys2 { - /// Evaluate a PDDL expression represented as a tree. /** * \param[in] node The root node of the PDDL expression. @@ -41,50 +40,39 @@ namespace plansys2 * \param[in] negate Invert the truth value. * \return result <- tuple(bool, bool, double) * result(0) true if success - * result(1) truth value of boolen expression + * result(1) truth value of boolean expression * result(2) value of numeric expression */ std::tuple evaluate( const plansys2_msgs::msg::Tree & tree, std::shared_ptr problem_client, - std::vector & predicates, - std::vector & functions, - bool apply = false, - bool use_state = false, - uint8_t node_id = 0, - bool negate = false); + std::vector & predicates, std::vector & functions, + bool apply = false, bool use_state = false, uint8_t node_id = 0, bool negate = false); std::tuple evaluate( const plansys2_msgs::msg::Tree & tree, - std::shared_ptr problem_client, - bool apply = false, + std::shared_ptr problem_client, bool apply = false, uint32_t node_id = 0); std::tuple evaluate( - const plansys2_msgs::msg::Tree & tree, - std::vector & predicates, - std::vector & functions, - bool apply = false, - uint32_t node_id = 0); + const plansys2_msgs::msg::Tree & tree, std::vector & predicates, + std::vector & functions, bool apply = false, uint32_t node_id = 0); /// Check a PDDL expression represented as a tree. /** -* \param[in] node The root node of the PDDL expression. -* \param[in] problem_client The problem expert client. -* \return ret Truth value of the PDDL expression. -* -* This function calls the evaluate function. -*/ + * \param[in] node The root node of the PDDL expression. + * \param[in] problem_client The problem expert client. + * \return ret Truth value of the PDDL expression. + * + * This function calls the evaluate function. + */ bool check( const plansys2_msgs::msg::Tree & tree, - std::shared_ptr problem_client, - uint32_t node_id = 0); + std::shared_ptr problem_client, uint32_t node_id = 0); bool check( - const plansys2_msgs::msg::Tree & tree, - std::vector & predicates, - std::vector & functions, - uint32_t node_id = 0); + const plansys2_msgs::msg::Tree & tree, std::vector & predicates, + std::vector & functions, uint32_t node_id = 0); /// Apply a PDDL expression represented as a tree. /** @@ -96,28 +84,25 @@ bool check( */ bool apply( const plansys2_msgs::msg::Tree & tree, - std::shared_ptr problem_client, - uint32_t node_id = 0); + std::shared_ptr problem_client, uint32_t node_id = 0); bool apply( - const plansys2_msgs::msg::Tree & tree, - std::vector & predicates, - std::vector & functions, - uint32_t node_id = 0); + const plansys2_msgs::msg::Tree & tree, std::vector & predicates, + std::vector & functions, uint32_t node_id = 0); /// Parse the action expression and time (optional) from an input string. /** -* \param[in] input The input string. -* \return result <- pair(string, int) -* result(0) The action expression. -* result(1) The action start time. -* -* The input string can have either of the following formats. -* "( ... )" -* "( ... ):