Imported Upstream version behaviortree-cpp 3.5.6 upstream/3.5.6
authorDongHun Kwak <dh0128.kwak@samsung.com>
Thu, 15 Apr 2021 01:23:16 +0000 (10:23 +0900)
committerDongHun Kwak <dh0128.kwak@samsung.com>
Thu, 15 Apr 2021 01:23:16 +0000 (10:23 +0900)
223 files changed:
.DS_Store [new file with mode: 0644]
.appveyor.yml [new file with mode: 0644]
.clang-format [new file with mode: 0644]
.github/FUNDING.yml [new file with mode: 0644]
.github/workflows/ros1.yaml [new file with mode: 0644]
.github/workflows/ros2.yaml [new file with mode: 0644]
.travis.yml [new file with mode: 0644]
3rdparty/filesystem/fwd.h [new file with mode: 0644]
3rdparty/filesystem/path.h [new file with mode: 0644]
3rdparty/filesystem/resolver.h [new file with mode: 0644]
3rdparty/minitrace/LICENSE [new file with mode: 0644]
3rdparty/minitrace/README.md [new file with mode: 0644]
3rdparty/minitrace/minitrace.cpp [new file with mode: 0644]
3rdparty/minitrace/minitrace.h [new file with mode: 0644]
CHANGELOG.rst [new file with mode: 0644]
CMakeLists.txt [new file with mode: 0644]
Doxyfile [new file with mode: 0644]
LICENSE [new file with mode: 0644]
MOOD2Be_final_report.pdf [new file with mode: 0644]
README.md [new file with mode: 0644]
cmake/FindZMQ.cmake [new file with mode: 0644]
conan/build.py [new file with mode: 0644]
conan/test_package/CMakeLists.txt [new file with mode: 0644]
conan/test_package/conanfile.py [new file with mode: 0644]
conan/test_package/test_package.cpp [new file with mode: 0644]
conan/travis/build.sh [new file with mode: 0755]
conan/travis/install.sh [new file with mode: 0755]
conanfile.py [new file with mode: 0644]
contributors.txt [new file with mode: 0644]
docs/BT_basics.md [new file with mode: 0644]
docs/DecoratorNode.md [new file with mode: 0644]
docs/FallbackNode.md [new file with mode: 0644]
docs/MigrationGuide.md [new file with mode: 0644]
docs/SequenceNode.md [new file with mode: 0644]
docs/getting_started.md [new file with mode: 0644]
docs/groot-screenshot.png [new file with mode: 0644]
docs/images/BT.png [new file with mode: 0644]
docs/images/CrossDoorSubtree.png [new file with mode: 0644]
docs/images/DecoratorEnterRoom.png [new file with mode: 0644]
docs/images/FallbackBasic.png [new file with mode: 0644]
docs/images/FallbackSimplified.png [new file with mode: 0644]
docs/images/FetchBeer.png [new file with mode: 0644]
docs/images/FetchBeer2.png [new file with mode: 0644]
docs/images/FetchBeerFails.png [new file with mode: 0644]
docs/images/LeafToComponentCommunication.png [new file with mode: 0644]
docs/images/ReactiveFallback.png [new file with mode: 0644]
docs/images/ReactiveSequence.png [new file with mode: 0644]
docs/images/ReadTheDocs.png [new file with mode: 0644]
docs/images/SequenceAll.png [new file with mode: 0644]
docs/images/SequenceBasic.png [new file with mode: 0644]
docs/images/SequenceNode.png [new file with mode: 0644]
docs/images/SequenceStar.png [new file with mode: 0644]
docs/images/TypeHierarchy.png [new file with mode: 0644]
docs/images/t06_remapping.png [new file with mode: 0644]
docs/index.md [new file with mode: 0644]
docs/robmosys_conformant_logo.png [new file with mode: 0644]
docs/tutorial_01_first_tree.md [new file with mode: 0644]
docs/tutorial_02_basic_ports.md [new file with mode: 0644]
docs/tutorial_03_generic_ports.md [new file with mode: 0644]
docs/tutorial_04_sequence_star.md [new file with mode: 0644]
docs/tutorial_05_subtrees.md [new file with mode: 0644]
docs/tutorial_06_subtree_ports.md [new file with mode: 0644]
docs/tutorial_07_legacy.md [new file with mode: 0644]
docs/tutorial_08_additional_args.md [new file with mode: 0644]
docs/tutorial_09_coroutines.md [new file with mode: 0644]
docs/tutorials_summary.md [new file with mode: 0644]
docs/uml/AllFallbacks.uxf [new file with mode: 0644]
docs/uml/AllSequences.uxf [new file with mode: 0644]
docs/uml/CrossDoorSubtree.uxf [new file with mode: 0644]
docs/uml/EnterRoom.uxf [new file with mode: 0644]
docs/uml/FallbackBasic.uxf [new file with mode: 0644]
docs/uml/FetchBeerFridge.uxf [new file with mode: 0644]
docs/uml/LeafToComponentCommunication.uxf [new file with mode: 0644]
docs/uml/Reactive.uxf [new file with mode: 0644]
docs/uml/ReadTheDocs.uxf [new file with mode: 0644]
docs/uml/TypeHierarchy.uxf [new file with mode: 0644]
docs/video_MOOD2Be.png [new file with mode: 0644]
docs/xml_format.md [new file with mode: 0644]
examples/CMakeLists.txt [new file with mode: 0644]
examples/broken_sequence.cpp [new file with mode: 0644]
examples/t01_build_your_first_tree.cpp [new file with mode: 0644]
examples/t02_basic_ports.cpp [new file with mode: 0644]
examples/t03_generic_ports.cpp [new file with mode: 0644]
examples/t04_reactive_sequence.cpp [new file with mode: 0644]
examples/t05_crossdoor.cpp [new file with mode: 0644]
examples/t06_subtree_port_remapping.cpp [new file with mode: 0644]
examples/t07_wrap_legacy.cpp [new file with mode: 0644]
examples/t08_additional_node_args.cpp [new file with mode: 0644]
examples/t09_async_actions_coroutines.cpp [new file with mode: 0644]
examples/t10_include_trees.cpp [new file with mode: 0644]
examples/t11_runtime_ports.cpp [new file with mode: 0644]
examples/t12_ncurses_manual_selector.cpp [new file with mode: 0644]
examples/test_files/Check.xml [new file with mode: 0644]
examples/test_files/subtree_test.xml [new file with mode: 0644]
examples/test_files/subtrees/Talk.xml [new file with mode: 0644]
include/behaviortree_cpp_v3/action_node.h [new file with mode: 0644]
include/behaviortree_cpp_v3/actions/always_failure_node.h [new file with mode: 0644]
include/behaviortree_cpp_v3/actions/always_success_node.h [new file with mode: 0644]
include/behaviortree_cpp_v3/actions/set_blackboard_node.h [new file with mode: 0644]
include/behaviortree_cpp_v3/basic_types.h [new file with mode: 0644]
include/behaviortree_cpp_v3/behavior_tree.h [new file with mode: 0644]
include/behaviortree_cpp_v3/blackboard.h [new file with mode: 0644]
include/behaviortree_cpp_v3/bt_factory.h [new file with mode: 0644]
include/behaviortree_cpp_v3/bt_parser.h [new file with mode: 0644]
include/behaviortree_cpp_v3/condition_node.h [new file with mode: 0644]
include/behaviortree_cpp_v3/control_node.h [new file with mode: 0644]
include/behaviortree_cpp_v3/controls/fallback_node.h [new file with mode: 0644]
include/behaviortree_cpp_v3/controls/if_then_else_node.h [new file with mode: 0644]
include/behaviortree_cpp_v3/controls/manual_node.h [new file with mode: 0644]
include/behaviortree_cpp_v3/controls/parallel_node.h [new file with mode: 0644]
include/behaviortree_cpp_v3/controls/reactive_fallback.h [new file with mode: 0644]
include/behaviortree_cpp_v3/controls/reactive_sequence.h [new file with mode: 0644]
include/behaviortree_cpp_v3/controls/sequence_node.h [new file with mode: 0644]
include/behaviortree_cpp_v3/controls/sequence_star_node.h [new file with mode: 0644]
include/behaviortree_cpp_v3/controls/switch_node.h [new file with mode: 0644]
include/behaviortree_cpp_v3/controls/while_do_else_node.h [new file with mode: 0644]
include/behaviortree_cpp_v3/decorator_node.h [new file with mode: 0644]
include/behaviortree_cpp_v3/decorators/blackboard_precondition.h [new file with mode: 0644]
include/behaviortree_cpp_v3/decorators/delay_node.h [new file with mode: 0644]
include/behaviortree_cpp_v3/decorators/force_failure_node.h [new file with mode: 0644]
include/behaviortree_cpp_v3/decorators/force_success_node.h [new file with mode: 0644]
include/behaviortree_cpp_v3/decorators/inverter_node.h [new file with mode: 0644]
include/behaviortree_cpp_v3/decorators/keep_running_until_failure_node.h [new file with mode: 0644]
include/behaviortree_cpp_v3/decorators/repeat_node.h [new file with mode: 0644]
include/behaviortree_cpp_v3/decorators/retry_node.h [new file with mode: 0644]
include/behaviortree_cpp_v3/decorators/subtree_node.h [new file with mode: 0644]
include/behaviortree_cpp_v3/decorators/timeout_node.h [new file with mode: 0644]
include/behaviortree_cpp_v3/decorators/timer_queue.h [new file with mode: 0644]
include/behaviortree_cpp_v3/exceptions.h [new file with mode: 0644]
include/behaviortree_cpp_v3/flatbuffers/BT_logger.fbs [new file with mode: 0644]
include/behaviortree_cpp_v3/flatbuffers/BT_logger_generated.h [new file with mode: 0644]
include/behaviortree_cpp_v3/flatbuffers/LICENSE.txt [new file with mode: 0644]
include/behaviortree_cpp_v3/flatbuffers/base.h [new file with mode: 0644]
include/behaviortree_cpp_v3/flatbuffers/bt_flatbuffer_helper.h [new file with mode: 0644]
include/behaviortree_cpp_v3/flatbuffers/flatbuffers.h [new file with mode: 0644]
include/behaviortree_cpp_v3/flatbuffers/stl_emulation.h [new file with mode: 0644]
include/behaviortree_cpp_v3/leaf_node.h [new file with mode: 0644]
include/behaviortree_cpp_v3/loggers/abstract_logger.h [new file with mode: 0644]
include/behaviortree_cpp_v3/loggers/bt_cout_logger.h [new file with mode: 0644]
include/behaviortree_cpp_v3/loggers/bt_file_logger.h [new file with mode: 0644]
include/behaviortree_cpp_v3/loggers/bt_minitrace_logger.h [new file with mode: 0644]
include/behaviortree_cpp_v3/loggers/bt_zmq_publisher.h [new file with mode: 0644]
include/behaviortree_cpp_v3/tree_node.h [new file with mode: 0644]
include/behaviortree_cpp_v3/utils/any.hpp [new file with mode: 0644]
include/behaviortree_cpp_v3/utils/convert_impl.hpp [new file with mode: 0644]
include/behaviortree_cpp_v3/utils/demangle_util.h [new file with mode: 0644]
include/behaviortree_cpp_v3/utils/expected.hpp [new file with mode: 0644]
include/behaviortree_cpp_v3/utils/make_unique.hpp [new file with mode: 0644]
include/behaviortree_cpp_v3/utils/platform.hpp [new file with mode: 0644]
include/behaviortree_cpp_v3/utils/safe_any.hpp [new file with mode: 0644]
include/behaviortree_cpp_v3/utils/shared_library.h [new file with mode: 0644]
include/behaviortree_cpp_v3/utils/signal.h [new file with mode: 0644]
include/behaviortree_cpp_v3/utils/simple_string.hpp [new file with mode: 0644]
include/behaviortree_cpp_v3/utils/strcat.hpp [new file with mode: 0644]
include/behaviortree_cpp_v3/utils/string_view.hpp [new file with mode: 0644]
include/behaviortree_cpp_v3/xml_parsing.h [new file with mode: 0644]
mkdocs.yml [new file with mode: 0644]
package.xml [new file with mode: 0644]
run_clang_format.sh [new file with mode: 0755]
sample_nodes/CMakeLists.txt [new file with mode: 0644]
sample_nodes/crossdoor_nodes.cpp [new file with mode: 0644]
sample_nodes/crossdoor_nodes.h [new file with mode: 0644]
sample_nodes/dummy_nodes.cpp [new file with mode: 0644]
sample_nodes/dummy_nodes.h [new file with mode: 0644]
sample_nodes/movebase_node.cpp [new file with mode: 0644]
sample_nodes/movebase_node.h [new file with mode: 0644]
src/action_node.cpp [new file with mode: 0644]
src/basic_types.cpp [new file with mode: 0644]
src/behavior_tree.cpp [new file with mode: 0644]
src/blackboard.cpp [new file with mode: 0644]
src/bt_factory.cpp [new file with mode: 0644]
src/condition_node.cpp [new file with mode: 0644]
src/control_node.cpp [new file with mode: 0644]
src/controls/fallback_node.cpp [new file with mode: 0644]
src/controls/if_then_else_node.cpp [new file with mode: 0644]
src/controls/manual_node.cpp [new file with mode: 0644]
src/controls/parallel_node.cpp [new file with mode: 0644]
src/controls/reactive_fallback.cpp [new file with mode: 0644]
src/controls/reactive_sequence.cpp [new file with mode: 0644]
src/controls/sequence_node.cpp [new file with mode: 0644]
src/controls/sequence_star_node.cpp [new file with mode: 0644]
src/controls/switch_node.cpp [new file with mode: 0644]
src/controls/while_do_else_node.cpp [new file with mode: 0644]
src/decorator_node.cpp [new file with mode: 0644]
src/decorators/delay_node.cpp [new file with mode: 0644]
src/decorators/inverter_node.cpp [new file with mode: 0644]
src/decorators/repeat_node.cpp [new file with mode: 0644]
src/decorators/retry_node.cpp [new file with mode: 0644]
src/decorators/subtree_node.cpp [new file with mode: 0644]
src/example.cpp [new file with mode: 0644]
src/loggers/bt_cout_logger.cpp [new file with mode: 0644]
src/loggers/bt_file_logger.cpp [new file with mode: 0644]
src/loggers/bt_minitrace_logger.cpp [new file with mode: 0644]
src/loggers/bt_zmq_publisher.cpp [new file with mode: 0644]
src/loggers/zmq.hpp [new file with mode: 0644]
src/private/tinyxml2.cpp [new file with mode: 0755]
src/private/tinyxml2.h [new file with mode: 0755]
src/shared_library.cpp [new file with mode: 0644]
src/shared_library_UNIX.cpp [new file with mode: 0644]
src/shared_library_WIN.cpp [new file with mode: 0644]
src/tree_node.cpp [new file with mode: 0644]
src/xml_parsing.cpp [new file with mode: 0644]
tests/CMakeLists.txt [new file with mode: 0644]
tests/gtest_blackboard.cpp [new file with mode: 0644]
tests/gtest_coroutines.cpp [new file with mode: 0644]
tests/gtest_decorator.cpp [new file with mode: 0644]
tests/gtest_factory.cpp [new file with mode: 0644]
tests/gtest_fallback.cpp [new file with mode: 0644]
tests/gtest_parallel.cpp [new file with mode: 0644]
tests/gtest_ports.cpp [new file with mode: 0644]
tests/gtest_sequence.cpp [new file with mode: 0644]
tests/gtest_subtree.cpp [new file with mode: 0644]
tests/gtest_switch.cpp [new file with mode: 0644]
tests/gtest_tree.cpp [new file with mode: 0644]
tests/include/action_test_node.h [new file with mode: 0644]
tests/include/condition_test_node.h [new file with mode: 0644]
tests/navigation_test.cpp [new file with mode: 0644]
tests/src/action_test_node.cpp [new file with mode: 0644]
tests/src/condition_test_node.cpp [new file with mode: 0644]
tools/CMakeLists.txt [new file with mode: 0644]
tools/bt_log_cat.cpp [new file with mode: 0644]
tools/bt_plugin_manifest.cpp [new file with mode: 0644]
tools/bt_recorder.cpp [new file with mode: 0644]

diff --git a/.DS_Store b/.DS_Store
new file mode 100644 (file)
index 0000000..5008ddf
Binary files /dev/null and b/.DS_Store differ
diff --git a/.appveyor.yml b/.appveyor.yml
new file mode 100644 (file)
index 0000000..0bb8c19
--- /dev/null
@@ -0,0 +1,22 @@
+clone_depth: 5
+
+environment:
+  matrix:
+    - GENERATOR : "Visual Studio 15 2017 Win64"
+      APPVEYOR_BUILD_WORKER_IMAGE: Visual Studio 2017
+      PLATFORM: x64
+
+      
+configuration:
+  - Release
+
+install:
+  - set PATH=C:\MinGW\bin;C:\MinGW\msys\1.0;%PATH%
+
+before_build:
+  - mkdir build
+  - cd build
+  - cmake "-G%GENERATOR%" -DCMAKE_IGNORE_PATH="C:/Program Files/Git/usr/bin" ..
+
+build_script:
+- cmake --build .
diff --git a/.clang-format b/.clang-format
new file mode 100644 (file)
index 0000000..d61f342
--- /dev/null
@@ -0,0 +1,68 @@
+---
+BasedOnStyle:  Google
+AccessModifierOffset: -2
+ConstructorInitializerIndentWidth: 2
+AlignEscapedNewlinesLeft: false
+AlignTrailingComments: true
+AllowAllParametersOfDeclarationOnNextLine: false
+AllowShortIfStatementsOnASingleLine: false
+AllowShortLoopsOnASingleLine: false
+AllowShortFunctionsOnASingleLine: None
+AllowShortLoopsOnASingleLine: false
+AlwaysBreakTemplateDeclarations: true
+AlwaysBreakBeforeMultilineStrings: false
+BreakBeforeBinaryOperators: false
+BreakBeforeTernaryOperators: false
+BreakConstructorInitializersBeforeComma: true
+BinPackParameters: true
+ColumnLimit:    100
+ConstructorInitializerAllOnOneLineOrOnePerLine: true
+DerivePointerBinding: false
+PointerBindsToType: true
+ExperimentalAutoDetectBinPacking: false
+IndentCaseLabels: true
+MaxEmptyLinesToKeep: 1
+NamespaceIndentation: None
+ObjCSpaceBeforeProtocolList: true
+PenaltyBreakBeforeFirstCallParameter: 19
+PenaltyBreakComment: 60
+PenaltyBreakString: 1
+PenaltyBreakFirstLessLess: 1000
+PenaltyExcessCharacter: 1000
+PenaltyReturnTypeOnItsOwnLine: 90
+SpacesBeforeTrailingComments: 3
+Cpp11BracedListStyle: true
+Standard:        Auto
+IndentWidth:     4
+TabWidth:        4
+UseTab:          Never
+IndentFunctionDeclarationAfterType: false
+SpacesInParentheses: false
+SpacesInAngles:  false
+SpaceInEmptyParentheses: false
+SpacesInCStyleCastParentheses: false
+SpaceAfterControlStatementKeyword: true
+SpaceBeforeAssignmentOperators: true
+ContinuationIndentWidth: 4
+SortIncludes: false
+SpaceAfterCStyleCast: false
+ReflowComments: false
+
+# Configure each individual brace in BraceWrapping
+BreakBeforeBraces: Custom
+
+# Control of individual brace wrapping cases
+BraceWrapping: {
+    AfterClass: 'true'
+    AfterControlStatement: 'true'
+    AfterEnum : 'true'
+    AfterFunction : 'true'
+    AfterNamespace : 'true'
+    AfterStruct : 'true'
+    AfterUnion : 'true'
+    BeforeCatch : 'true'
+    BeforeElse : 'true'
+    IndentBraces : 'false'
+}
+...
+
diff --git a/.github/FUNDING.yml b/.github/FUNDING.yml
new file mode 100644 (file)
index 0000000..accb08c
--- /dev/null
@@ -0,0 +1,4 @@
+# These are supported funding model platforms
+
+github: facontidavide
+custom: https://www.paypal.me/facontidavide
diff --git a/.github/workflows/ros1.yaml b/.github/workflows/ros1.yaml
new file mode 100644 (file)
index 0000000..e6d2f9b
--- /dev/null
@@ -0,0 +1,18 @@
+name: ros1
+
+on: [push, pull_request]
+
+jobs:
+  industrial_ci:
+    strategy:
+      matrix:
+        env:
+          - {ROS_DISTRO: melodic, ROS_REPO: main}
+          - {ROS_DISTRO: kinetic, ROS_REPO: main}
+    runs-on: ubuntu-latest
+    steps:
+      - uses: actions/checkout@v1
+      - uses: 'ros-industrial/industrial_ci@master'
+        env: ${{matrix.env}}
+        with:
+            package-name: behaviortree_cpp_v3
diff --git a/.github/workflows/ros2.yaml b/.github/workflows/ros2.yaml
new file mode 100644 (file)
index 0000000..9f8c5e7
--- /dev/null
@@ -0,0 +1,17 @@
+name: ros2
+
+on: [push, pull_request]
+
+jobs:
+  industrial_ci:
+    strategy:
+      matrix:
+        env:
+          - {ROS_DISTRO: eloquent, ROS_REPO: main}
+    runs-on: ubuntu-latest
+    steps:
+      - uses: actions/checkout@v1
+      - uses: 'ros-industrial/industrial_ci@master'
+        env: ${{matrix.env}}
+        with:
+            package-name: behaviortree_cpp_v3
diff --git a/.travis.yml b/.travis.yml
new file mode 100644 (file)
index 0000000..c0d1fdb
--- /dev/null
@@ -0,0 +1,66 @@
+# This config file for Travis CI utilizes ros-industrial/industrial_ci package.
+# For more info for the package, see https://github.com/ros-industrial/industrial_ci/blob/master/README.rst
+
+sudo: required
+dist: xenial
+language: cpp
+
+os:
+  - linux
+
+compiler:
+  - gcc
+
+conan-linux: &conan-linux
+    os: linux
+    dist: xenial
+    language: python
+    python: "3.7"
+    services:
+      - docker
+    before_install:
+      - true
+    install:
+      - ./conan/travis/install.sh
+    script:
+      - ./conan/travis/build.sh
+
+conan-osx: &conan-osx
+    os: osx
+    language: generic
+    before_install:
+      - true
+    install:
+      - ./conan/travis/install.sh
+    script:
+      - ./conan/travis/build.sh
+
+matrix:
+    include:
+      - bare_linux:
+        env: ROS_DISTRO="none"
+    fast_finish: false
+
+before_install:
+  - sudo apt-get update && sudo apt-get --reinstall install -qq build-essential
+  - if [ "$ROS_DISTRO" = "none" ]; then sudo apt-get --reinstall install -qq libzmq3-dev libdw-dev; fi
+  # GTest: see motivation here https://www.eriksmistad.no/getting-started-with-google-test-on-ubuntu/
+  - sudo apt-get --reinstall install -qq libgtest-dev cmake
+  - cd /usr/src/gtest
+  - sudo cmake CMakeLists.txt
+  - sudo make
+  - sudo cp *.a /usr/lib
+  - cd $TRAVIS_BUILD_DIR
+
+install:
+  - if [ "$ROS_DISTRO" != "none" ]; then git clone https://github.com/ros-industrial/industrial_ci.git .ci_config; fi
+
+before_script:
+  # Prepare build directory
+  - mkdir -p build
+
+script:
+  - if [ "$ROS_DISTRO"  = "none" ]; then (cd build; cmake .. ; sudo cmake --build . --target install; ./bin/behaviortree_cpp_v3_test); fi
+  - if [ "$ROS_DISTRO" != "none" ]; then (.ci_config/travis.sh); fi
+
+
diff --git a/3rdparty/filesystem/fwd.h b/3rdparty/filesystem/fwd.h
new file mode 100644 (file)
index 0000000..3552199
--- /dev/null
@@ -0,0 +1,24 @@
+/*
+    fwd.h -- Forward declarations for path.h and resolver.h
+
+    Copyright (c) 2015 Wenzel Jakob <wenzel@inf.ethz.ch>
+
+    All rights reserved. Use of this source code is governed by a
+    BSD-style license that can be found in the LICENSE file.
+*/
+
+#pragma once
+
+#if !defined(NAMESPACE_BEGIN)
+#define NAMESPACE_BEGIN(name) namespace name {
+#endif
+#if !defined(NAMESPACE_END)
+#define NAMESPACE_END(name) }
+#endif
+
+NAMESPACE_BEGIN(filesystem)
+
+class path;
+class resolver;
+
+NAMESPACE_END(filesystem)
diff --git a/3rdparty/filesystem/path.h b/3rdparty/filesystem/path.h
new file mode 100644 (file)
index 0000000..a5b582c
--- /dev/null
@@ -0,0 +1,339 @@
+/*
+    path.h -- A simple class for manipulating paths on Linux/Windows/Mac OS
+
+    Copyright (c) 2015 Wenzel Jakob <wenzel@inf.ethz.ch>
+
+    All rights reserved. Use of this source code is governed by a
+    BSD-style license that can be found in the LICENSE file.
+*/
+
+#pragma once
+
+#include "fwd.h"
+#include <string>
+#include <vector>
+#include <stdexcept>
+#include <sstream>
+#include <cctype>
+#include <cstdlib>
+#include <cerrno>
+#include <cstring>
+
+#if defined(_WIN32)
+# include <windows.h>
+#else
+# include <unistd.h>
+#endif
+#include <sys/stat.h>
+
+#if defined(__linux)
+# include <linux/limits.h>
+#endif
+
+NAMESPACE_BEGIN(filesystem)
+
+/**
+ * \brief Simple class for manipulating paths on Linux/Windows/Mac OS
+ *
+ * This class is just a temporary workaround to avoid the heavy boost
+ * dependency until boost::filesystem is integrated into the standard template
+ * library at some point in the future.
+ */
+class path {
+public:
+    enum path_type {
+        windows_path = 0,
+        posix_path = 1,
+#if defined(_WIN32)
+        native_path = windows_path
+#else
+        native_path = posix_path
+#endif
+    };
+
+    path() : m_type(native_path), m_absolute(false) { }
+
+    path(const path &path)
+        : m_type(path.m_type), m_path(path.m_path), m_absolute(path.m_absolute) {}
+
+    path(path &&path)
+        : m_type(path.m_type), m_path(std::move(path.m_path)),
+          m_absolute(path.m_absolute) {}
+
+    path(const char *string) { set(string); }
+
+    path(const std::string &string) { set(string); }
+
+#if defined(_WIN32)
+    path(const std::wstring &wstring) { set(wstring); }
+    path(const wchar_t *wstring) { set(wstring); }
+#endif
+
+    size_t length() const { return m_path.size(); }
+
+    bool empty() const { return m_path.empty(); }
+
+    bool is_absolute() const { return m_absolute; }
+
+    path make_absolute() const {
+#if !defined(_WIN32)
+        char temp[PATH_MAX];
+        if (realpath(str().c_str(), temp) == NULL)
+            throw std::runtime_error("Internal error in realpath(): " + std::string(strerror(errno)));
+        return path(temp);
+#else
+        std::wstring value = wstr(), out(MAX_PATH, '\0');
+        DWORD length = GetFullPathNameW(value.c_str(), MAX_PATH, &out[0], NULL);
+        if (length == 0)
+            throw std::runtime_error("Internal error in realpath(): " + std::to_string(GetLastError()));
+        return path(out.substr(0, length));
+#endif
+    }
+
+    bool exists() const {
+#if defined(_WIN32)
+        return GetFileAttributesW(wstr().c_str()) != INVALID_FILE_ATTRIBUTES;
+#else
+        struct stat sb;
+        return stat(str().c_str(), &sb) == 0;
+#endif
+    }
+
+    size_t file_size() const {
+#if defined(_WIN32)
+        struct _stati64 sb;
+        if (_wstati64(wstr().c_str(), &sb) != 0)
+            throw std::runtime_error("path::file_size(): cannot stat file \"" + str() + "\"!");
+#else
+        struct stat sb;
+        if (stat(str().c_str(), &sb) != 0)
+            throw std::runtime_error("path::file_size(): cannot stat file \"" + str() + "\"!");
+#endif
+        return (size_t) sb.st_size;
+    }
+
+    bool is_directory() const {
+#if defined(_WIN32)
+        DWORD result = GetFileAttributesW(wstr().c_str());
+        if (result == INVALID_FILE_ATTRIBUTES)
+            return false;
+        return (result & FILE_ATTRIBUTE_DIRECTORY) != 0;
+#else
+        struct stat sb;
+        if (stat(str().c_str(), &sb))
+            return false;
+        return S_ISDIR(sb.st_mode);
+#endif
+    }
+
+    bool is_file() const {
+#if defined(_WIN32)
+        DWORD attr = GetFileAttributesW(wstr().c_str());
+        return (attr != INVALID_FILE_ATTRIBUTES && (attr & FILE_ATTRIBUTE_DIRECTORY) == 0);
+#else
+        struct stat sb;
+        if (stat(str().c_str(), &sb))
+            return false;
+        return S_ISREG(sb.st_mode);
+#endif
+    }
+
+    std::string extension() const {
+        const std::string &name = filename();
+        size_t pos = name.find_last_of(".");
+        if (pos == std::string::npos)
+            return "";
+        return name.substr(pos+1);
+    }
+
+    std::string filename() const {
+        if (empty())
+            return "";
+        const std::string &last = m_path[m_path.size()-1];
+        return last;
+    }
+
+    path parent_path() const {
+        path result;
+        result.m_absolute = m_absolute;
+
+        if (m_path.empty()) {
+            if (!m_absolute)
+                result.m_path.push_back("..");
+        } else {
+            size_t until = m_path.size() - 1;
+            for (size_t i = 0; i < until; ++i)
+                result.m_path.push_back(m_path[i]);
+        }
+        return result;
+    }
+
+    path operator/(const path &other) const {
+        if (other.m_absolute)
+            throw std::runtime_error("path::operator/(): expected a relative path!");
+        if (m_type != other.m_type)
+            throw std::runtime_error("path::operator/(): expected a path of the same type!");
+
+        path result(*this);
+
+        for (size_t i=0; i<other.m_path.size(); ++i)
+            result.m_path.push_back(other.m_path[i]);
+
+        return result;
+    }
+
+    std::string str(path_type type = native_path) const {
+        std::ostringstream oss;
+
+        if (m_type == posix_path && m_absolute)
+            oss << "/";
+
+        for (size_t i=0; i<m_path.size(); ++i) {
+            oss << m_path[i];
+            if (i+1 < m_path.size()) {
+                if (type == posix_path)
+                    oss << '/';
+                else
+                    oss << '\\';
+            }
+        }
+
+        return oss.str();
+    }
+
+    void set(const std::string &str, path_type type = native_path) {
+        m_type = type;
+        if (type == windows_path) {
+            m_path = tokenize(str, "/\\");
+            m_absolute = str.size() >= 2 && std::isalpha(str[0]) && str[1] == ':';
+        } else {
+            m_path = tokenize(str, "/");
+            m_absolute = !str.empty() && str[0] == '/';
+        }
+    }
+
+    path &operator=(const path &path) {
+        m_type = path.m_type;
+        m_path = path.m_path;
+        m_absolute = path.m_absolute;
+        return *this;
+    }
+
+    path &operator=(path &&path) {
+        if (this != &path) {
+            m_type = path.m_type;
+            m_path = std::move(path.m_path);
+            m_absolute = path.m_absolute;
+        }
+        return *this;
+    }
+
+    friend std::ostream &operator<<(std::ostream &os, const path &path) {
+        os << path.str();
+        return os;
+    }
+
+    bool remove_file() {
+#if !defined(_WIN32)
+        return std::remove(str().c_str()) == 0;
+#else
+        return DeleteFileW(wstr().c_str()) != 0;
+#endif
+    }
+
+    bool resize_file(size_t target_length) {
+#if !defined(_WIN32)
+        return ::truncate(str().c_str(), (off_t) target_length) == 0;
+#else
+        HANDLE handle = CreateFileW(wstr().c_str(), GENERIC_WRITE, 0, nullptr, 0, FILE_ATTRIBUTE_NORMAL, nullptr);
+        if (handle == INVALID_HANDLE_VALUE)
+            return false;
+        LARGE_INTEGER size;
+        size.QuadPart = (LONGLONG) target_length;
+        if (SetFilePointerEx(handle, size, NULL, FILE_BEGIN) == 0) {
+            CloseHandle(handle);
+            return false;
+        }
+        if (SetEndOfFile(handle) == 0) {
+            CloseHandle(handle);
+            return false;
+        }
+        CloseHandle(handle);
+        return true;
+#endif
+    }
+
+    static path getcwd() {
+#if !defined(_WIN32)
+        char temp[PATH_MAX];
+        if (::getcwd(temp, PATH_MAX) == NULL)
+            throw std::runtime_error("Internal error in getcwd(): " + std::string(strerror(errno)));
+        return path(temp);
+#else
+        std::wstring temp(MAX_PATH, '\0');
+        if (!_wgetcwd(&temp[0], MAX_PATH))
+            throw std::runtime_error("Internal error in getcwd(): " + std::to_string(GetLastError()));
+        return path(temp.c_str());
+#endif
+    }
+
+#if defined(_WIN32)
+    std::wstring wstr(path_type type = native_path) const {
+        std::string temp = str(type);
+        int size = MultiByteToWideChar(CP_UTF8, 0, &temp[0], (int)temp.size(), NULL, 0);
+        std::wstring result(size, 0);
+        MultiByteToWideChar(CP_UTF8, 0, &temp[0], (int)temp.size(), &result[0], size);
+        return result;
+    }
+
+
+    void set(const std::wstring &wstring, path_type type = native_path) {
+        std::string string;
+        if (!wstring.empty()) {
+            int size = WideCharToMultiByte(CP_UTF8, 0, &wstring[0], (int)wstring.size(),
+                            NULL, 0, NULL, NULL);
+            string.resize(size, 0);
+            WideCharToMultiByte(CP_UTF8, 0, &wstring[0], (int)wstring.size(),
+                                &string[0], size, NULL, NULL);
+        }
+        set(string, type);
+    }
+
+    path &operator=(const std::wstring &str) { set(str); return *this; }
+#endif
+
+    bool operator==(const path &p) const { return p.m_path == m_path; }
+    bool operator!=(const path &p) const { return p.m_path != m_path; }
+
+protected:
+    static std::vector<std::string> tokenize(const std::string &string, const std::string &delim) {
+        std::string::size_type lastPos = 0, pos = string.find_first_of(delim, lastPos);
+        std::vector<std::string> tokens;
+
+        while (lastPos != std::string::npos) {
+            if (pos != lastPos)
+                tokens.push_back(string.substr(lastPos, pos - lastPos));
+            lastPos = pos;
+            if (lastPos == std::string::npos || lastPos + 1 == string.length())
+                break;
+            pos = string.find_first_of(delim, ++lastPos);
+        }
+
+        return tokens;
+    }
+
+protected:
+    path_type m_type;
+    std::vector<std::string> m_path;
+    bool m_absolute;
+};
+
+inline bool create_directory(const path& p) {
+#if defined(_WIN32)
+    return CreateDirectoryW(p.wstr().c_str(), NULL) != 0;
+#else
+    return mkdir(p.str().c_str(), S_IRUSR | S_IWUSR | S_IXUSR) == 0;
+#endif
+}
+
+NAMESPACE_END(filesystem)
diff --git a/3rdparty/filesystem/resolver.h b/3rdparty/filesystem/resolver.h
new file mode 100644 (file)
index 0000000..0c75768
--- /dev/null
@@ -0,0 +1,72 @@
+/*
+    resolver.h -- A simple class for cross-platform path resolution
+
+    Copyright (c) 2015 Wenzel Jakob <wenzel@inf.ethz.ch>
+
+    All rights reserved. Use of this source code is governed by a
+    BSD-style license that can be found in the LICENSE file.
+*/
+
+#pragma once
+
+#include "path.h"
+
+NAMESPACE_BEGIN(filesystem)
+
+/**
+ * \brief Simple class for resolving paths on Linux/Windows/Mac OS
+ *
+ * This convenience class looks for a file or directory given its name
+ * and a set of search paths. The implementation walks through the
+ * search paths in order and stops once the file is found.
+ */
+class resolver {
+public:
+    typedef std::vector<path>::iterator iterator;
+    typedef std::vector<path>::const_iterator const_iterator;
+
+    resolver() {
+        m_paths.push_back(path::getcwd());
+    }
+
+    size_t size() const { return m_paths.size(); }
+
+    iterator begin() { return m_paths.begin(); }
+    iterator end()   { return m_paths.end(); }
+
+    const_iterator begin() const { return m_paths.begin(); }
+    const_iterator end()   const { return m_paths.end(); }
+
+    void erase(iterator it) { m_paths.erase(it); }
+
+    void prepend(const path &path) { m_paths.insert(m_paths.begin(), path); }
+    void append(const path &path) { m_paths.push_back(path); }
+    const path &operator[](size_t index) const { return m_paths[index]; }
+    path &operator[](size_t index) { return m_paths[index]; }
+
+    path resolve(const path &value) const {
+        for (const_iterator it = m_paths.begin(); it != m_paths.end(); ++it) {
+            path combined = *it / value;
+            if (combined.exists())
+                return combined;
+        }
+        return value;
+    }
+
+    friend std::ostream &operator<<(std::ostream &os, const resolver &r) {
+        os << "resolver[" << std::endl;
+        for (size_t i = 0; i < r.m_paths.size(); ++i) {
+            os << "  \"" << r.m_paths[i] << "\"";
+            if (i + 1 < r.m_paths.size())
+                os << ",";
+            os << std::endl;
+        }
+        os << "]";
+        return os;
+    }
+
+private:
+    std::vector<path> m_paths;
+};
+
+NAMESPACE_END(filesystem)
diff --git a/3rdparty/minitrace/LICENSE b/3rdparty/minitrace/LICENSE
new file mode 100644 (file)
index 0000000..f746931
--- /dev/null
@@ -0,0 +1,21 @@
+The MIT License (MIT)
+
+Copyright (c) 2014 Henrik RydgÃ¥rd
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
\ No newline at end of file
diff --git a/3rdparty/minitrace/README.md b/3rdparty/minitrace/README.md
new file mode 100644 (file)
index 0000000..70e7179
--- /dev/null
@@ -0,0 +1,103 @@
+minitrace
+=========
+by Henrik RydgÃ¥rd 2014 (hrydgard+minitrace@gmail.com)
+
+MIT licensed, feel free to use however you want. If you use it for something cool, I'd love to hear about it!
+
+This is a C library with C++ helpers for producing JSON traces suitable for Chrome's excellent built-in trace viewer (chrome://tracing).
+
+Extremely simple to build and use. Tested on Mac and Windows, but should compile anywhere you can use ANSI C with few or no changes.
+
+Sample output (see example code below):
+
+![minitrace](http://www.ppsspp.org/img/minitrace.png)
+
+Remember to be careful when interpreting the output. This is not a sampling profiler, so it only records start and stop times for blocks. This means that blocks grow even when the CPU is off running another thread, and that it can look like work is being done on more blocks at a time than you have CPUs.
+
+
+How to use
+----------
+
+  1. Include minitrace.c and minitrace.h in your project. #include minitrace.h in some common header.
+
+  2. In your initialization code:
+
+        mtr_init("trace.json");
+
+  3. In your exit code:
+
+        mtr_shutdown();
+
+  4. In all functions you want to profile:
+
+        // C
+        MTR_BEGIN("GFX", "RasterizeTriangle")
+        ...
+        MTR_END("GFX", "RasterizeTriangle")
+
+        // C++
+        MTR_SCOPE("GFX", "RasterizeTriangle")
+
+  5. In Google Chrome open "about:tracing"
+
+  6. Click Open, and choose your trace.json
+
+  7. Navigate the trace view using the WASD keys, and Look for bottlenecks and optimize your application. 
+
+  8. In your final release build, build with
+
+         -DMTR_DISABLE
+
+
+By default, it will collect 1 million tracepoints and then stop. You can change this behaviour, see the
+top of the header file.
+
+Note: Please only use string literals in MTR statements.
+
+Example code
+------------
+
+    int main(int argc, const char *argv[]) {
+      int i;
+      mtr_init("trace.json");
+
+      MTR_META_PROCESS_NAME("minitrace_test");
+      MTR_META_THREAD_NAME("main thread");
+
+      int long_running_thing_1;
+      int long_running_thing_2;
+
+      MTR_START("background", "long_running", &long_running_thing_1);
+      MTR_START("background", "long_running", &long_running_thing_2);
+
+      MTR_BEGIN("main", "outer");
+      usleep(80000);
+      for (i = 0; i < 3; i++) {
+        MTR_BEGIN("main", "inner");
+        usleep(40000);
+        MTR_END("main", "inner");
+        usleep(10000);
+      }
+      MTR_STEP("background", "long_running", &long_running_thing_1, "middle step");
+      usleep(80000);
+      MTR_END("main", "outer");
+
+      usleep(50000);
+      MTR_INSTANT("main", "the end");
+      usleep(10000);
+      MTR_FINISH("background", "long_running", &long_running_thing_1);
+      MTR_FINISH("background", "long_running", &long_running_thing_2);
+
+      mtr_flush();
+      mtr_shutdown();
+      return 0;
+    }
+
+The output will result in something looking a little like the picture at the top of this readme.
+
+Future plans:
+
+  * Builtin background flush thread support with better synchronization, no more fixed limit
+  * Support for more trace arguments, more tracing types
+
+If you use this, feel free to tell me how, and what issues you may have had. hrydgard+minitrace@gmail.com
diff --git a/3rdparty/minitrace/minitrace.cpp b/3rdparty/minitrace/minitrace.cpp
new file mode 100644 (file)
index 0000000..b6daa54
--- /dev/null
@@ -0,0 +1,379 @@
+// minitrace
+// Copyright 2014 by Henrik RydgÃ¥rd
+// http://www.github.com/hrydgard/minitrace
+// Released under the MIT license.
+
+// See minitrace.h for basic documentation.
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+
+#ifdef _WIN32
+#pragma warning (disable:4996)
+#define WIN32_LEAN_AND_MEAN
+#include <windows.h>
+#define __thread __declspec(thread)
+#define pthread_mutex_t CRITICAL_SECTION
+#define pthread_mutex_init(a, b) InitializeCriticalSection(a)
+#define pthread_mutex_lock(a) EnterCriticalSection(a)
+#define pthread_mutex_unlock(a) LeaveCriticalSection(a)
+#define pthread_mutex_destroy(a) DeleteCriticalSection(a)
+#else
+#include <signal.h>
+#include <pthread.h>
+#include <sys/time.h>
+#include <unistd.h>
+#endif
+
+#include "minitrace.h"
+
+#define ARRAY_SIZE(x) sizeof(x)/sizeof(x[0])
+
+namespace minitrace {
+
+// Ugh, this struct is already pretty heavy.
+// Will probably need to move arguments to a second buffer to support more than one.
+typedef struct raw_event {
+       const char *name;
+       const char *cat;
+       void *id;
+       int64_t ts;
+       uint32_t pid;
+       uint32_t tid;
+       char ph;
+       mtr_arg_type arg_type;
+       const char *arg_name;
+       union {
+               const char *a_str;
+               int a_int;
+               double a_double;
+       };
+} raw_event_t;
+
+static raw_event_t *buffer;
+static volatile int count;
+static int is_tracing = 0;
+static int64_t time_offset;
+static int first_line = 1;
+static FILE *file;
+static __thread int cur_thread_id;     // Thread local storage
+static pthread_mutex_t mutex;
+
+#define STRING_POOL_SIZE 100
+static char *str_pool[100];
+
+// Tiny portability layer.
+// Exposes:
+//      get_cur_thread_id()
+//      mtr_time_s()
+//      pthread basics
+#ifdef _WIN32
+static int get_cur_thread_id() {
+       return (int)GetCurrentThreadId();
+}
+
+static uint64_t _frequency = 0;
+static uint64_t _starttime = 0;
+
+inline int64_t mtr_time_usec(){
+  static int64_t prev = 0;
+       if (_frequency == 0) {
+               QueryPerformanceFrequency((LARGE_INTEGER*)&_frequency);
+               QueryPerformanceCounter((LARGE_INTEGER*)&_starttime);
+       }
+       __int64 time;
+       QueryPerformanceCounter((LARGE_INTEGER*)&time);
+    int64_t now = static_cast<int64_t>( 1.0e6 * ((double)(time - _starttime) / (double)_frequency));
+  if( now <= prev) now = prev + 1;
+  prev = now;
+  return now;
+}
+
+// Ctrl+C handling for Windows console apps
+static BOOL WINAPI CtrlHandler(DWORD fdwCtrlType) {
+       if (is_tracing && fdwCtrlType == CTRL_C_EVENT) {
+               printf("Ctrl-C detected! Flushing trace and shutting down.\n\n");
+               mtr_flush();
+               mtr_shutdown();
+       }
+       ExitProcess(1);
+}
+
+void mtr_register_sigint_handler() {
+       // For console apps:
+       SetConsoleCtrlHandler(&CtrlHandler, TRUE);
+}
+
+#else
+
+static inline int get_cur_thread_id() {
+       return (int)(intptr_t)pthread_self();
+}
+
+#if defined(BLACKBERRY)
+inline int64_t mtr_time_usec(){
+  static int64_t prev = 0;
+       struct timespec time;
+       clock_gettime(CLOCK_MONOTONIC, &time); // Linux must use CLOCK_MONOTONIC_RAW due to time warps
+  int64_t now = time.tv_sec*1000000 + time.tv_nsec / 1000;
+  if( now <= prev) now = prev + 1;
+  prev = now;
+  return now;
+}
+#else
+int64_t mtr_time_usec()
+{
+  static int64_t prev = 0;
+       struct timeval tv;
+    gettimeofday(&tv, nullptr);
+  int64_t now = 1000000*tv.tv_sec + tv.tv_usec;
+  if( now <= prev) now = prev + 1;
+  prev = now;
+  return now;
+}
+#endif // !BLACKBERRY
+
+static void termination_handler(int ) {
+       if (is_tracing) {
+               printf("Ctrl-C detected! Flushing trace and shutting down.\n\n");
+               mtr_flush();
+        fwrite("\n]}\n", 1, 4, file);
+        fclose(file);
+       }
+       exit(1);
+}
+
+void mtr_register_sigint_handler() {
+#ifndef MTR_ENABLED
+       return;
+#endif
+       // Avoid altering set-to-be-ignored handlers while registering.
+       if (signal(SIGINT, &termination_handler) == SIG_IGN)
+               signal(SIGINT, SIG_IGN);
+}
+
+#endif
+
+void mtr_init(const char *json_file) {
+#ifndef MTR_ENABLED
+       return;
+#endif
+       buffer = (raw_event_t *)malloc(INTERNAL_MINITRACE_BUFFER_SIZE * sizeof(raw_event_t));
+       is_tracing = 1;
+       count = 0;
+    file = fopen(json_file, "wb");
+       const char *header = "{\"traceEvents\":[\n";
+    fwrite(header, 1, strlen(header), file);
+    time_offset = mtr_time_usec();
+       first_line = 1;
+       pthread_mutex_init(&mutex, 0);
+}
+
+void mtr_shutdown() {
+       int i;
+#ifndef MTR_ENABLED
+       return;
+#endif
+       is_tracing = 0;
+       mtr_flush();
+    fwrite("\n]}\n", 1, 4, file);
+    fclose(file);
+       pthread_mutex_destroy(&mutex);
+    file = 0;
+       free(buffer);
+       buffer = 0;
+       for (i = 0; i < STRING_POOL_SIZE; i++) {
+               if (str_pool[i]) {
+                       free(str_pool[i]);
+                       str_pool[i] = 0;
+               }
+       }
+}
+
+const char *mtr_pool_string(const char *str) {
+       int i;
+       for (i = 0; i < STRING_POOL_SIZE; i++) {
+               if (!str_pool[i]) {
+      str_pool[i] = (char *)malloc(strlen(str) + 1);
+                       strcpy(str_pool[i], str);
+                       return str_pool[i];
+               } else {
+                       if (!strcmp(str, str_pool[i]))
+                               return str_pool[i];
+               }
+       }
+       return "string pool full";
+}
+
+void mtr_start() {
+#ifndef MTR_ENABLED
+       return;
+#endif
+       is_tracing = 1;
+}
+
+void mtr_stop() {
+#ifndef MTR_ENABLED
+       return;
+#endif
+       is_tracing = 0;
+}
+
+// TODO: fwrite more than one line at a time.
+void mtr_flush() {
+#ifndef MTR_ENABLED
+       return;
+#endif
+       char linebuf[1024];
+       char arg_buf[256];
+       char id_buf[256];
+       // We have to lock while flushing. So we really should avoid flushing as much as possible.
+
+
+       pthread_mutex_lock(&mutex);
+       int old_tracing = is_tracing;
+       is_tracing = 0; // Stop logging even if using interlocked increments instead of the mutex. Can cause data loss.
+
+       for (int i = 0; i < count; i++) {
+               raw_event_t *raw = &buffer[i];
+               int len;
+               switch (raw->arg_type) {
+               case MTR_ARG_TYPE_INT:
+                       snprintf(arg_buf, ARRAY_SIZE(arg_buf), "\"%s\":%i", raw->arg_name, raw->a_int);
+                       break;
+               case MTR_ARG_TYPE_STRING_CONST:
+                       snprintf(arg_buf, ARRAY_SIZE(arg_buf), "\"%s\":\"%s\"", raw->arg_name, raw->a_str);
+                       break;
+               case MTR_ARG_TYPE_STRING_COPY:
+                       if (strlen(raw->a_str) > 700) {
+                               ((char*)raw->a_str)[700] = 0;
+                       }
+                       snprintf(arg_buf, ARRAY_SIZE(arg_buf), "\"%s\":\"%s\"", raw->arg_name, raw->a_str);
+                       break;
+               case MTR_ARG_TYPE_NONE:
+               default:
+                       arg_buf[0] = '\0';
+                       break;
+               }
+               if (raw->id) {
+                       switch (raw->ph) {
+                       case 'S':
+                       case 'T':
+                       case 'F':
+                               // TODO: Support full 64-bit pointers
+                               snprintf(id_buf, ARRAY_SIZE(id_buf), ",\"id\":\"0x%08x\"", (uint32_t)(uintptr_t)raw->id);
+                               break;
+                       case 'X':
+                               snprintf(id_buf, ARRAY_SIZE(id_buf), ",\"dur\":%i", (int)raw->a_double);
+                               break;
+                       }
+               } else {
+                       id_buf[0] = 0;
+               }
+               const char *cat = raw->cat;
+#ifdef _WIN32
+               // On Windows, we often end up with backslashes in category.
+               {
+                       char temp[256];
+                       int cat_len = (int)strlen(cat);
+            if (cat_len > 255)
+                cat_len = 255;
+            for (int a = 0; a < cat_len; a++)
+            {
+                               temp[a] = cat[a] == '\\' ? '/' : cat[a];
+                       }
+            temp[cat_len] = 0;
+                       cat = temp;
+               }
+#endif
+
+               len = snprintf(linebuf, ARRAY_SIZE(linebuf), "%s{\"cat\":\"%s\",\"pid\":%i,\"tid\":%i,\"ts\":%" PRId64 ",\"ph\":\"%c\",\"name\":\"%s\",\"args\":{%s}%s}",
+                               first_line ? "" : ",\n",
+                               cat, raw->pid, raw->tid, raw->ts - time_offset, raw->ph, raw->name, arg_buf, id_buf);
+        fwrite(linebuf, 1, len, file);
+        fflush(file);
+               first_line = 0;
+       }
+       count = 0;
+       is_tracing = old_tracing;
+       pthread_mutex_unlock(&mutex);
+}
+
+void internal_mtr_raw_event(const char *category, const char *name, char ph, void *id) {
+#ifndef MTR_ENABLED
+       return;
+#endif
+       if (!is_tracing || count >= INTERNAL_MINITRACE_BUFFER_SIZE)
+               return;
+  int64_t ts = mtr_time_usec();
+       if (!cur_thread_id) {
+               cur_thread_id = get_cur_thread_id();
+       }
+
+#if 0 && _WIN32        // TODO: This needs testing
+       int bufPos = InterlockedIncrement(&count);
+       raw_event_t *ev = &buffer[count - 1];
+#else
+       pthread_mutex_lock(&mutex);
+       raw_event_t *ev = &buffer[count];
+       count++;
+       pthread_mutex_unlock(&mutex);
+#endif
+
+       ev->cat = category;
+       ev->name = name;
+       ev->id = id;
+       ev->ph = ph;
+  if (ev->ph == 'X') {
+    int64_t x;
+    memcpy(&x, id, sizeof(int64_t));
+    ev->ts = x;
+    ev->a_double = static_cast<double>(ts - x);
+  } else {
+    ev->ts = ts;
+  }
+  ev->tid = cur_thread_id;
+       ev->pid = 0;
+}
+
+void internal_mtr_raw_event_arg(const char *category, const char *name, char ph, void *id, mtr_arg_type arg_type, const char *arg_name, void *arg_value) {
+#ifndef MTR_ENABLED
+       return;
+#endif
+       if (!is_tracing || count >= INTERNAL_MINITRACE_BUFFER_SIZE)
+               return;
+       if (!cur_thread_id) {
+               cur_thread_id = get_cur_thread_id();
+       }
+  int64_t ts = mtr_time_usec();
+
+#if 0 && _WIN32        // TODO: This needs testing
+       int bufPos = InterlockedIncrement(&count);
+       raw_event_t *ev = &buffer[count - 1];
+#else
+       pthread_mutex_lock(&mutex);
+       raw_event_t *ev = &buffer[count];
+       count++;
+       pthread_mutex_unlock(&mutex);
+#endif
+
+       ev->cat = category;
+       ev->name = name;
+       ev->id = id;
+  ev->ts = ts;
+       ev->ph = ph;
+       ev->tid = cur_thread_id;
+       ev->pid = 0;
+       ev->arg_type = arg_type;
+       ev->arg_name = arg_name;
+       switch (arg_type) {
+       case MTR_ARG_TYPE_INT: ev->a_int = (int)(uintptr_t)arg_value; break;
+       case MTR_ARG_TYPE_STRING_CONST: ev->a_str = (const char*)arg_value; break;
+       case MTR_ARG_TYPE_STRING_COPY: ev->a_str = strdup((const char*)arg_value); break;
+       default:
+               break;
+       }
+}
+
+}
diff --git a/3rdparty/minitrace/minitrace.h b/3rdparty/minitrace/minitrace.h
new file mode 100644 (file)
index 0000000..c7d5b31
--- /dev/null
@@ -0,0 +1,267 @@
+#ifndef MINITRACE_H
+#define MINITRACE_H
+
+// Minitrace
+//
+// Copyright 2014 by Henrik RydgÃ¥rd
+// http://www.github.com/hrydgard/minitrace
+// Released under the MIT license.
+//
+// Ultra-light dependency free library for performance tracing C/C++ applications.
+// Produces traces compatible with Google Chrome's trace viewer.
+// Simply open "about:tracing" in Chrome and load the produced JSON.
+//
+// This contains far less template magic than the original libraries from Chrome
+// because this is meant to be usable from C.
+//
+// See README.md for a tutorial.
+//
+// The trace format is documented here:
+// https://docs.google.com/document/d/1CvAClvFfyA5R-PhYUmn5OOQtYMH4h6I0nSsKchNAySU/edit
+// More:
+// http://www.altdevblogaday.com/2012/08/21/using-chrometracing-to-view-your-inline-profiling-data/
+
+#include <inttypes.h>
+
+#define MTR_ENABLED
+
+// If MTR_ENABLED is not defined, Minitrace does nothing and has near zero overhead.
+// Preferably, set this flag in your build system. If you can't just uncomment this line.
+// #define MTR_ENABLED
+
+// By default, will collect up to 1000000 events, then you must flush.
+// It's recommended that you simply call mtr_flush on a background thread
+// occasionally. It's safe...ish.
+#define INTERNAL_MINITRACE_BUFFER_SIZE 1000000
+
+//#define MTR_ENABLED
+
+namespace minitrace {
+
+// Initializes Minitrace. Must be called very early during startup of your executable,
+// before any MTR_ statements..
+void mtr_init(const char *json_file);
+
+// Shuts down minitrace cleanly, flushing the trace buffer.
+void mtr_shutdown();
+
+// Lets you enable and disable Minitrace at runtime.
+// May cause strange discontinuities in the output.
+// Minitrace is enabled on startup by default.
+void mtr_start();
+void mtr_stop();
+
+// Flushes the collected data to disk, clearing the buffer for new data.
+void mtr_flush();
+
+// Returns the current time in seconds. Used internally by Minitrace. No caching.
+int64_t mtr_time_usec();
+
+// Registers a handler that will flush the trace on Ctrl+C.
+// Works on Linux and MacOSX, and in Win32 console applications.
+void mtr_register_sigint_handler();
+
+// Utility function that should rarely be used.
+// If str is semi dynamic, store it permanently in a small pool so we don't need to malloc it.
+// The pool fills up fast though and performance isn't great.
+// Returns a fixed string if the pool is full.
+const char *mtr_pool_string(const char *str);
+
+// Commented-out types will be supported in the future.
+typedef enum {
+       MTR_ARG_TYPE_NONE = 0,
+       MTR_ARG_TYPE_INT = 1,   // I
+       // MTR_ARG_TYPE_FLOAT = 2,  // TODO
+       // MTR_ARG_TYPE_DOUBLE = 3,  // TODO
+       MTR_ARG_TYPE_STRING_CONST = 8,  // C
+       MTR_ARG_TYPE_STRING_COPY = 9,
+       // MTR_ARG_TYPE_JSON_COPY = 10,
+} mtr_arg_type;
+
+// TODO: Add support for more than one argument (metadata) per event
+// Having more costs speed and memory.
+#define MTR_MAX_ARGS 1
+
+// Only use the macros to call these.
+void internal_mtr_raw_event(const char *category, const char *name, char ph, void *id);
+void internal_mtr_raw_event_arg(const char *category, const char *name, char ph, void *id, mtr_arg_type arg_type, const char *arg_name, void *arg_value);
+
+#ifdef MTR_ENABLED
+
+// c - category. Can be filtered by in trace viewer (or at least that's the intention).
+//     A good use is to pass __FILE__, there are macros further below that will do it for you.
+// n - name. Pass __FUNCTION__ in most cases, unless you are marking up parts of one.
+
+// Scopes. In C++, use MTR_SCOPE. In C, always match them within the same scope.
+#define MTR_BEGIN(c, n) internal_mtr_raw_event(c, n, 'B', nullptr)
+#define MTR_END(c, n) internal_mtr_raw_event(c, n, 'E', nullptr)
+#define MTR_SCOPE(c, n) MTRScopedTrace ____mtr_scope(c, n)
+#define MTR_SCOPE_LIMIT(c, n, l) MTRScopedTraceLimit ____mtr_scope(c, n, l)
+
+// Async events. Can span threads. ID identifies which events to connect in the view.
+#define MTR_START(c, n, id) internal_mtr_raw_event(c, n, 'S', (void *)(id))
+#define MTR_STEP(c, n, id, step) internal_mtr_raw_event_arg(c, n, 'T', (void *)(id), MTR_ARG_TYPE_STRING_CONST, "step", (void *)(step))
+#define MTR_FINISH(c, n, id) internal_mtr_raw_event(c, n, 'F', (void *)(id))
+
+// Flow events. Like async events, but displayed in a more fancy way in the viewer.
+#define MTR_FLOW_START(c, n, id) internal_mtr_raw_event(c, n, 's', (void *)(id))
+#define MTR_FLOW_STEP(c, n, id, step) internal_mtr_raw_event_arg(c, n, 't', (void *)(id), MTR_ARG_TYPE_STRING_CONST, "step", (void *)(step))
+#define MTR_FLOW_FINISH(c, n, id) internal_mtr_raw_event(c, n, 'f', (void *)(id))
+
+// The same macros, but with a single named argument which shows up as metadata in the viewer.
+// _I for int.
+// _C is for a const string arg.
+// _S will copy the string, freeing on flush (expensive but sometimes necessary).
+// but required if the string was generated dynamically.
+
+// Note that it's fine to match BEGIN_S with END and BEGIN with END_S, etc.
+#define MTR_BEGIN_C(c, n, aname, astrval) internal_mtr_raw_event_arg(c, n, 'B', 0, MTR_ARG_TYPE_STRING_CONST, aname, (void *)(astrval))
+#define MTR_END_C(c, n, aname, astrval) internal_mtr_raw_event_arg(c, n, 'E', 0, MTR_ARG_TYPE_STRING_CONST, aname, (void *)(astrval))
+#define MTR_SCOPE_C(c, n, aname, astrval) MTRScopedTraceArg ____mtr_scope(c, n, MTR_ARG_TYPE_STRING_CONST, aname, (void *)(astrval))
+
+#define MTR_BEGIN_S(c, n, aname, astrval) internal_mtr_raw_event_arg(c, n, 'B', 0, MTR_ARG_TYPE_STRING_COPY, aname, (void *)(astrval))
+#define MTR_END_S(c, n, aname, astrval) internal_mtr_raw_event_arg(c, n, 'E', 0, MTR_ARG_TYPE_STRING_COPY, aname, (void *)(astrval))
+#define MTR_SCOPE_S(c, n, aname, astrval) MTRScopedTraceArg ____mtr_scope(c, n, MTR_ARG_TYPE_STRING_COPY, aname, (void *)(astrval))
+
+#define MTR_BEGIN_I(c, n, aname, aintval) internal_mtr_raw_event_arg(c, n, 'B', 0, MTR_ARG_TYPE_INT, aname, (void*)(intptr_t)(aintval))
+#define MTR_END_I(c, n, aname, aintval) internal_mtr_raw_event_arg(c, n, 'E', 0, MTR_ARG_TYPE_INT, aname, (void*)(intptr_t)(aintval))
+#define MTR_SCOPE_I(c, n, aname, aintval) MTRScopedTraceArg ____mtr_scope(c, n, MTR_ARG_TYPE_INT, aname, (void*)(intptr_t)(aintval))
+
+// Instant events. For things with no duration.
+#define MTR_INSTANT(c, n) internal_mtr_raw_event(c, n, 'I', nullptr)
+#define MTR_INSTANT_C(c, n, aname, astrval) internal_mtr_raw_event(c, n, 'I', 0, MTR_ARG_TYPE_STRING_CONST, aname, (void *)(astrval))
+#define MTR_INSTANT_I(c, n, aname, aintval) internal_mtr_raw_event(c, n, 'I', 0, MTR_ARG_TYPE_INT, aname, (void *)(aintval))
+
+// Counters (can't do multi-value counters yet)
+#define MTR_COUNTER(c, n, val) internal_mtr_raw_event_arg(c, n, 'C', 0, MTR_ARG_TYPE_INT, n, (void *)(intptr_t)(val))
+
+// Metadata. Call at the start preferably. Must be const strings.
+
+#define MTR_META_PROCESS_NAME(n) internal_mtr_raw_event_arg("", "process_name", 'M', 0, MTR_ARG_TYPE_STRING_COPY, "name", (void *)(n))
+#define MTR_META_THREAD_NAME(n) internal_mtr_raw_event_arg("", "thread_name", 'M', 0, MTR_ARG_TYPE_STRING_COPY, "name", (void *)(n))
+#define MTR_META_THREAD_SORT_INDEX(i) internal_mtr_raw_event_arg("", "thread_sort_index", 'M', 0, MTR_ARG_TYPE_INT, "sort_index", (void *)(i))
+
+#else
+
+#define MTR_BEGIN(c, n)
+#define MTR_END(c, n)
+#define MTR_SCOPE(c, n)
+#define MTR_START(c, n, id)
+#define MTR_STEP(c, n, id, step)
+#define MTR_FINISH(c, n, id)
+#define MTR_FLOW_START(c, n, id)
+#define MTR_FLOW_STEP(c, n, id, step)
+#define MTR_FLOW_FINISH(c, n, id)
+#define MTR_INSTANT(c, n)
+
+#define MTR_BEGIN_C(c, n, aname, astrval)
+#define MTR_END_C(c, n, aname, astrval)
+#define MTR_SCOPE_C(c, n, aname, astrval)
+
+#define MTR_BEGIN_S(c, n, aname, astrval)
+#define MTR_END_S(c, n, aname, astrval)
+#define MTR_SCOPE_S(c, n, aname, astrval)
+
+#define MTR_BEGIN_I(c, n, aname, aintval)
+#define MTR_END_I(c, n, aname, aintval)
+#define MTR_SCOPE_I(c, n, aname, aintval)
+
+#define MTR_INSTANT(c, n)
+#define MTR_INSTANT_C(c, n, aname, astrval)
+#define MTR_INSTANT_I(c, n, aname, aintval)
+
+// Counters (can't do multi-value counters yet)
+#define MTR_COUNTER(c, n, val)
+
+// Metadata. Call at the start preferably. Must be const strings.
+
+#define MTR_META_PROCESS_NAME(n)
+
+#define MTR_META_THREAD_NAME(n)
+#define MTR_META_THREAD_SORT_INDEX(i)
+
+#endif
+
+// Shortcuts for simple function timing with automatic categories and names.
+
+#define MTR_BEGIN_FUNC() MTR_BEGIN(__FILE__, __FUNCTION__)
+#define MTR_END_FUNC() MTR_END(__FILE__, __FUNCTION__)
+#define MTR_SCOPE_FUNC() MTR_SCOPE(__FILE__, __FUNCTION__)
+#define MTR_INSTANT_FUNC() MTR_INSTANT(__FILE__, __FUNCTION__)
+#define MTR_SCOPE_FUNC_LIMIT_S(l) MTRScopedTraceLimit ____mtr_scope(__FILE__, __FUNCTION__, l)
+#define MTR_SCOPE_FUNC_LIMIT_MS(l) MTRScopedTraceLimit ____mtr_scope(__FILE__, __FUNCTION__, 1)
+
+// Same, but with a single argument of the usual types.
+#define MTR_BEGIN_FUNC_S(aname, arg) MTR_BEGIN_S(__FILE__, __FUNCTION__, aname, arg)
+#define MTR_END_FUNC_S(aname, arg) MTR_END_S(__FILE__, __FUNCTION__, aname, arg)
+#define MTR_SCOPE_FUNC_S(aname, arg) MTR_SCOPE_S(__FILE__, __FUNCTION__, aname, arg)
+
+#define MTR_BEGIN_FUNC_C(aname, arg) MTR_BEGIN_C(__FILE__, __FUNCTION__, aname, arg)
+#define MTR_END_FUNC_C(aname, arg) MTR_END_C(__FILE__, __FUNCTION__, aname, arg)
+#define MTR_SCOPE_FUNC_C(aname, arg) MTR_SCOPE_C(__FILE__, __FUNCTION__, aname, arg)
+
+#define MTR_BEGIN_FUNC_I(aname, arg) MTR_BEGIN_I(__FILE__, __FUNCTION__, aname, arg)
+#define MTR_END_FUNC_I(aname, arg) MTR_END_I(__FILE__, __FUNCTION__, aname, arg)
+#define MTR_SCOPE_FUNC_I(aname, arg) MTR_SCOPE_I(__FILE__, __FUNCTION__, aname, arg)
+
+
+#ifdef MTR_ENABLED
+// These are optimized to use X events (combined B and E). Much easier to do in C++ than in C.
+class MTRScopedTrace {
+public:
+       MTRScopedTrace(const char *category, const char *name)
+               : category_(category), name_(name) {
+    start_time_ = mtr_time_usec();
+       }
+       ~MTRScopedTrace() {
+               internal_mtr_raw_event(category_, name_, 'X', &start_time_);
+       }
+
+private:
+       const char *category_;
+       const char *name_;
+    int64_t start_time_;
+};
+
+// Only outputs a block if execution time exceeded the limit.
+// TODO: This will effectively call mtr_time_usec twice at the end, which is bad.
+class MTRScopedTraceLimit {
+public:
+    MTRScopedTraceLimit(const char* category, const char* name, int64_t limit_s)
+               : category_(category), name_(name), limit_(limit_s) {
+    start_time_ = mtr_time_usec();
+       }
+       ~MTRScopedTraceLimit() {
+    int64_t end_time = mtr_time_usec();
+               if (end_time - start_time_ >= limit_) {
+                       internal_mtr_raw_event(category_, name_, 'X', &start_time_);
+               }
+       }
+
+private:
+       const char *category_;
+       const char *name_;
+    int64_t start_time_;
+    int64_t limit_;
+};
+
+class MTRScopedTraceArg {
+public:
+       MTRScopedTraceArg(const char *category, const char *name, mtr_arg_type arg_type, const char *arg_name, void *arg_value)
+               : category_(category), name_(name) {
+               internal_mtr_raw_event_arg(category, name, 'B', 0, arg_type, arg_name, arg_value);
+       }
+       ~MTRScopedTraceArg() {
+               internal_mtr_raw_event(category_, name_, 'E', 0);
+       }
+
+private:
+       const char *category_;
+       const char *name_;
+};
+
+#endif
+
+} //end namespace
+
+#endif
diff --git a/CHANGELOG.rst b/CHANGELOG.rst
new file mode 100644 (file)
index 0000000..270ee83
--- /dev/null
@@ -0,0 +1,325 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package behaviortree_cpp
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+3.5.6 (2021-02-03)
+------------------
+* fix issue `#227 <https://github.com/BehaviorTree/BehaviorTree.CPP/issues/227>`_
+* fix issue `#256 <https://github.com/BehaviorTree/BehaviorTree.CPP/issues/256>`_
+* Merge branch 'master' of https://github.com/BehaviorTree/BehaviorTree.CPP
+* fix issue `#250 <https://github.com/BehaviorTree/BehaviorTree.CPP/issues/250>`_
+* Fixed typos on SequenceNode.md (`#254 <https://github.com/BehaviorTree/BehaviorTree.CPP/issues/254>`_)
+* Contributors: Davide Faconti, LucasNolasco
+
+3.5.5 (2021-01-27)
+------------------
+* fix issue `#251 <https://github.com/BehaviorTree/BehaviorTree.CPP/issues/251>`_
+* Contributors: Davide Faconti
+
+3.5.4 (2020-12-10)
+------------------
+* Update bt_factory.cpp (`#245 <https://github.com/BehaviorTree/BehaviorTree.CPP/issues/245>`_)
+* Use the latest version of zmq.hpp
+* Improved switching BTs with active Groot monitoring (ZMQ logger destruction) (`#244 <https://github.com/BehaviorTree/BehaviorTree.CPP/issues/244>`_)
+  * Skip 100ms (max) wait for detached thread
+  * add {} to single line if statements
+* Update retry_node.cpp
+* fix
+* fix issue `#230 <https://github.com/BehaviorTree/BehaviorTree.CPP/issues/230>`_
+* Contributors: Davide Faconti, Florian Gramß, amangiat88
+
+3.5.3 (2020-09-10)
+------------------
+* fix issue `#228 <https://github.com/BehaviorTree/BehaviorTree.CPP/issues/228>`_ . Retry and Repeat node need to halt the child
+* better tutorial
+* Contributors: Davide Faconti
+
+3.5.2 (2020-09-02)
+------------------
+* fix warning and follow coding standard
+* docs: Small changes to tutorial 02 (`#225 <https://github.com/BehaviorTree/BehaviorTree.CPP/issues/225>`_)
+  Co-authored-by: Valerio Magnago <valerio.magnago@fraunhofer.it>
+* Merge branch 'master' of https://github.com/BehaviorTree/BehaviorTree.CPP
+* tutorial 1 fixed
+* decreasing warning level to fix issue `#220 <https://github.com/BehaviorTree/BehaviorTree.CPP/issues/220>`_
+* fix compilation
+* Allow BT factory to define clock source for TimerQueue/TimerNode (`#215 <https://github.com/BehaviorTree/BehaviorTree.CPP/issues/215>`_)
+  * Allow BT factory to define clock source for TimerQueue/TimerNode
+  * Fix unit tests
+  Co-authored-by: Cam Fulton <cfulton@symbotic.com>
+  Co-authored-by: Davide Faconti <davide.faconti@gmail.com>
+* Added delay node and wait for enter keypress node (`#182 <https://github.com/BehaviorTree/BehaviorTree.CPP/issues/182>`_)
+  * Added delay node and wait for enter press node
+  * Fixed unsigned int to int conversion bug
+  * Added a new timer to keep a track of delay timeout and return RUNNING in the meanwhile
+  * Removed wait for keypress node
+  * Review changes suggested by gramss
+  Co-authored-by: Indraneel Patil <indraneel.p@greyorange.com>
+* Update SequenceNode.md (`#211 <https://github.com/BehaviorTree/BehaviorTree.CPP/issues/211>`_)
+* add failure threshold to parallel node with tests (`#216 <https://github.com/BehaviorTree/BehaviorTree.CPP/issues/216>`_)
+* Update tutorial_05_subtrees.md
+  I believe that the API has been updated. Reflecting the same in this tutorial.
+* Contributors: Aayush Naik, Davide Faconti, Indraneel Patil, Renan Salles, Valerio Magnago, Wuqiqi123, fultoncjb
+
+3.5.1 (2020-06-11)
+------------------
+* trying to fix compilation in eloquent  Minor fix on line 19
+* Update README.md
+* more badges
+* readme updated
+* fix ros2 compilation?
+* move to github actions
+* replace dot by zero in boost version (`#197 <https://github.com/BehaviorTree/BehaviorTree.CPP/issues/197>`_)
+* Always use nonstd::string_view for binary compatibility (fix issue `#200 <https://github.com/BehaviorTree/BehaviorTree.CPP/issues/200>`_)
+* Adding ForceRunningNode Decorator (`#192 <https://github.com/BehaviorTree/BehaviorTree.CPP/issues/192>`_)
+* updated doc
+* Add XML parsing support for custom Control Nodes (`#194 <https://github.com/BehaviorTree/BehaviorTree.CPP/issues/194>`_)
+* Fix typo
+* [Windows] Compare `std::type_info` objects to check type. (`#181 <https://github.com/BehaviorTree/BehaviorTree.CPP/issues/181>`_)
+* Fix pseudocode for ReactiveFallback. (`#191 <https://github.com/BehaviorTree/BehaviorTree.CPP/issues/191>`_)
+* Contributors: Aayush Naik, Darío Hereñú, Davide Faconti, Francisco Martín Rico, G.Doisy, Sarathkrishnan Ramesh, Sean Yen, Ting Chang
+
+3.5.0 (2020-05-14)
+------------------
+* added IfThenElse and  WhileDoElse
+* issue `#190 <https://github.com/BehaviorTree/BehaviorTree.CPP/issues/190>`_
+* unit test added
+* reverting to a better solution
+* RemappedSubTree added
+* Fix issue `#188 <https://github.com/BehaviorTree/BehaviorTree.CPP/issues/188>`_
+* added function const std::string& key (issue `#183 <https://github.com/BehaviorTree/BehaviorTree.CPP/issues/183>`_)
+* Contributors: Davide Faconti, daf@blue-ocean-robotics.com
+
+* added IfThenElse and  WhileDoElse
+* issue `#190 <https://github.com/BehaviorTree/BehaviorTree.CPP/issues/190>`_
+* unit test added
+* reverting to a better solution
+* RemappedSubTree added
+* Fix issue `#188 <https://github.com/BehaviorTree/BehaviorTree.CPP/issues/188>`_
+* added function const std::string& key (issue `#183 <https://github.com/BehaviorTree/BehaviorTree.CPP/issues/183>`_)
+* Contributors: Davide Faconti, daf@blue-ocean-robotics.com
+
+3.1.1 (2019-11-10)
+------------------
+* fix samples compilation (hopefully)
+* Contributors: Davide Faconti
+
+3.1.0 (2019-10-30)
+------------------
+* Error message corrected
+* fix windows and mingw compilation (?)
+* Merge pull request `#70 <https://github.com/BehaviorTree/BehaviorTree.CPP/issues/70>`_ from Masadow/patch-3
+  Added 32bits compilation configuration for msvc
+* make Tree non copyable
+* fix `#114 <https://github.com/BehaviorTree/BehaviorTree.CPP/issues/114>`_
+* Merge branch 'master' of https://github.com/BehaviorTree/BehaviorTree.CPP
+* critical bug fix affecting AsyncActionNode
+  When a Tree is copied, all the thread related to AsyncActionNode where
+  invoked.
+  As a consequence, they are never executed, despite the fact that the
+  value RUNNING is returned.
+* Fix issue `#109 <https://github.com/BehaviorTree/BehaviorTree.CPP/issues/109>`_
+* fix `#111 <https://github.com/BehaviorTree/BehaviorTree.CPP/issues/111>`_
+* Merge pull request `#108 <https://github.com/BehaviorTree/BehaviorTree.CPP/issues/108>`_ from daniel-serrano/add-RobMoSys-acknowledgement
+  Add robmosys acknowledgement
+* Add robomosys acknowledgement as requested
+* Add robomosys acknowledgement as requested
+* added more comments (issue `#102 <https://github.com/BehaviorTree/BehaviorTree.CPP/issues/102>`_)
+* Update README.md
+* Add files via upload
+* Merge pull request `#96 <https://github.com/BehaviorTree/BehaviorTree.CPP/issues/96>`_ from LoyVanBeek/patch-1
+  Fix typo
+* Update tutorial_04_sequence_star.md
+* fix compilation
+* removing backward_cpp
+  Motivation: backward_cpp is SUPER useful, but it is a library to use at
+  the application level. It makes no sense to add it at the library level.
+* Merge pull request `#95 <https://github.com/BehaviorTree/BehaviorTree.CPP/issues/95>`_ from LoyVanBeek/patch-1
+  Remove 0 in front of http://... URL to publication
+* Remove 0 in front of http://... URL to publication
+  Hopefully, this makes the link correctly click-able when rendered to HTML
+* fix issue `#84 <https://github.com/BehaviorTree/BehaviorTree.CPP/issues/84>`_ (Directories)
+* add infinite loop to Repeat and Retry (issue `#80 <https://github.com/BehaviorTree/BehaviorTree.CPP/issues/80>`_)
+* fix unit test
+* issue `#82 <https://github.com/BehaviorTree/BehaviorTree.CPP/issues/82>`_
+* fix issue `#82 <https://github.com/BehaviorTree/BehaviorTree.CPP/issues/82>`_
+* Added 32bits compilation configuration for msvc
+* Contributors: Daniel Serrano, Davide Facont, Davide Faconti, Jimmy Delas, Loy
+
+3.0.7 (2019-04-02)
+------------------
+* this should fix issue with tinyXML2 once and for all (maybe...)
+* improvement #79
+* doc fix
+* Deprecating <remap> tag in SubTree
+* fix windows compilation
+* Update README.md
+* back to c++11
+* Contributors: Davide Faconti, Ferran Roure
+
+3.0.4 (2019-03-19)
+------------------
+* fix issue #72 with sibling subtrees
+* Update .travis.yml
+* Contributors: Davide Faconti
+
+3.0.3 (2019-03-12)
+------------------
+* moving to C++14... deal with it
+* updated tinyXML2. Should fix several issues too
+* add "d" to debug library on Windows
+* fixed compilation error on Windows x64 (issue #63)
+* Improved MSVC compilation
+  Added _CRT_SECURE_NO_WARNINGS flag for msvc compilation
+* adding TreeNode::modifyPortsRemapping that might be useful in the future
+* Merge pull request #64 from luminize/patch-1
+  docs/xml_format.md
+* Merge pull request #65 from luminize/patch-2
+  docs/tutorial_01_first_tree.md: fix typo
+* docs/tutorial_01_first_tree.md: fix typo
+* fix compilation in Windows/Release
+* remove a warning in Windows
+* Update README.md
+* Merge branch 'windows_compilation'
+* fix issue #63 : compile on windows
+* Update .travis.yml
+* Create .appveyor.yml
+* fix compilation on windows
+* fix potential issue
+* bug fix
+* Update README.md
+* Contributors: Bas de Bruijn,  Davide Faconti, Jimmy Delas, hlzl
+
+3.0.2 (2019-03-04)
+------------------
+* make flatbuffers visible to other project (such as Groot)
+* docs fix
+* Contributors: Davide Faconti
+
+3.0.0 (2019-02-27)
+------------------
+* Merge branch 'ver_3'. Too many changes to count...
+* Contributors: Davide Facont, Davide Faconti, ImgBotApp, Victor Lopez
+
+2.5.1 (2019-01-14)
+------------------
+* fix installation directory
+* #39 Fix Conan version (#42)
+  Signed-off-by: Uilian Ries <uilianries@gmail.com>
+* Update .travis.yml
+* Conan package distribution (#39)
+* Non-functional refactoring of xml_parsing to clean up the code
+* cosmetic changes in the code of BehaviorTreeFactory
+* XML schema. Related to enchancement #40
+* call setRegistrationName() for built-in Nodes
+  The methos is called by BehaviorTreefactory, therefore it
+  registrationName is empty if trees are created programmatically.
+* Reset reference count when destroying logger (issue #38)
+* Contributors: Davide Facont, Davide Faconti, Uilian Ries
+
+2.5.0 (2018-12-12)
+------------------
+* Introducing SyncActionNode that is more self explaining and less ambiguous
+* fix potential problem related to ControlNode::haltChildren()
+* Adding example/test of navigation and recovery behavior. Related to issue #36
+* Contributors: Davide Faconti
+
+2.4.4 (2018-12-12)
+------------------
+* adding virtual TreeNode::onInit() [issue #33]
+* fix issue #34 : if you don't implement convertFromString, it will compile but it may throw
+* Pretty demangled names and obsolate comments removed
+* bug fixes
+* more comments
+* [enhancement #32]: add CoroActionNode and rename ActionNode as "AsynActionNode"
+  The name ActionNode was confusing and it has been deprecated.
+* Update README.md
+* removed old file
+* Fix issue #31 : convertFromString mandatory for TreeNode::getParam, not Blackboard::get
+* Cherry piking changes from PR #19 which solve issue #2 CONAN support
+* Contributors: Davide Faconti
+
+2.4.3 (2018-12-07)
+------------------
+* Merge branch 'master' into ros2
+* removed old file
+* Fix issue #31 : convertFromString mandatory for TreeNode::getParam, not Blackboard::get
+* 2.4.3
+* version bump
+* Merge pull request #30 from nuclearsandwich/patch-1
+  Fix typo in package name.
+* Remove extra find_package(ament_cmake_gtest).
+  This package should only be needed if BUILD_TESTING is on and is
+  find_package'd below if ament_cmake is found and BUILD_TESTING is on.
+* Fix typo in package name.
+* added video to readme
+* Cherry piking changes from PR #19 which solve issue #2 CONAN support
+* Merge pull request #29 from nuclearsandwich/ament-gtest-dep
+  Add test dependency on ament_cmake_gtest.
+* Add test dependency on ament_cmake_gtest.
+* fix travis removing CI
+* Contributors: Davide Faconti, Steven! Ragnarök
+
+2.4.2 (2018-12-05)
+------------------
+* support ament
+* change to ament
+* Contributors: Davide Faconti
+
+2.4.1 (2018-12-05)
+------------------
+* fix warnings and dependencies in ROS, mainly related to ZMQ
+* Contributors: Davide Faconti
+
+2.4.0 (2018-12-05)
+------------------
+* Merge pull request #27 from mjeronimo/bt-12-4-2018
+  Add support for ament/colcon build
+* updated documentation
+* Merge pull request #25 from BehaviorTree/include_xml
+  Add the ability to include an XML from another one
+* <include> supports ROS package getPath (issue #17)
+* Trying to fix writeXML (issue #24)
+* New feature: include XMl from other XMLs (issue #17)
+* more verbose error message
+* adding unit tests for Repeat and Retry nodes #23
+* Bug fix in Retry and Repeat Decorators (needs unit test)
+* Throw if the parameter in blackboard can't be read
+* Try to prevent error #22 in user code
+* changed the protocol of the XML
+* fixing issue #22
+* Contributors: Davide Faconti, Michael Jeronimo
+
+2.3.0 (2018-11-28)
+------------------
+* Fix: registerBuilder did not register the manifest. It was "broken" as public API method
+* Use the Pimpl idiom to hide zmq from the header file
+* move header of minitrace in the cpp file
+* Fixed a crash occuring when you didn't initialized a Tree object (#20)
+* Fix issue #16
+* add ParallelNode to pre-registered entries in factory (issue #13)
+* removed M_PI
+* Update the documentation
+* Contributors: Davide Faconti, Jimmy Delas
+
+2.2.0 (2018-11-20)
+------------------
+* fix typo
+* method contains() added to BlackBoard
+* back compatible API change to improve the wrapping of legacy code (issue #15)
+  Eventually, SimpleAction, SimpleDecorators and SimpleCondition can use
+  blackboard and NodeParameters too.
+* reduce potential memory allocations using string_view
+* fix important issue with SubtreeNode
+* Read at every tick the parameter if Blackboard is used
+* Adding NodeParameters to ParallelNode
+* travis update
+* merge pull request #14 related to #10 (with some minor changes)
+* Fix issue #8 and warning reported in #4
+  Fixed problem of visibility with TinyXML2
+* Contributors: Davide Faconti, Uilian Ries 
+
+2.1.0 (2018-11-16)
+------------------
+* version 2.1. New directory structure
+* Contributors: Davide Faconti
diff --git a/CMakeLists.txt b/CMakeLists.txt
new file mode 100644 (file)
index 0000000..a390aed
--- /dev/null
@@ -0,0 +1,281 @@
+cmake_minimum_required(VERSION 3.5.1) # version on Ubuntu Xenial
+project(behaviortree_cpp_v3)
+
+#---- Add the subdirectory cmake ----
+set(CMAKE_CONFIG_PATH ${CMAKE_MODULE_PATH}  "${CMAKE_CURRENT_LIST_DIR}/cmake")
+list(APPEND CMAKE_MODULE_PATH "${CMAKE_CONFIG_PATH}")
+
+#---- Enable C++14 ----
+set(CMAKE_CXX_STANDARD 14)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+if(MSVC)
+    add_definitions(-D_CRT_SECURE_NO_WARNINGS)
+endif()
+
+#---- Include boost to add coroutines ----
+find_package(Boost COMPONENTS coroutine QUIET)
+
+if(Boost_FOUND)
+    string(REPLACE "." "0" Boost_VERSION_NODOT ${Boost_VERSION})
+    if(NOT Boost_VERSION_NODOT VERSION_LESS 105900)
+        message(STATUS "Found boost::coroutine2.")
+        add_definitions(-DBT_BOOST_COROUTINE2)
+        set(BT_COROUTINES true)
+    elseif(NOT Boost_VERSION_NODOT VERSION_LESS 105300)
+        message(STATUS "Found boost::coroutine.")
+        add_definitions(-DBT_BOOST_COROUTINE)
+        set(BT_COROUTINES true)
+    endif()
+    include_directories(${Boost_INCLUDE_DIRS})
+endif()
+
+
+if(NOT DEFINED BT_COROUTINES)
+    message(STATUS "Coroutines disabled. Install Boost to enable them (version 1.59+ recommended).")
+    add_definitions(-DBT_NO_COROUTINES)
+endif()
+
+set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+
+#---- project configuration ----
+option(BUILD_EXAMPLES   "Build tutorials and examples" ON)
+option(BUILD_UNIT_TESTS "Build the unit tests" ON)
+option(BUILD_TOOLS "Build commandline tools" ON)
+option(BUILD_SHARED_LIBS "Build shared libraries" ON)
+
+#---- Find other packages ----
+find_package(Threads)
+find_package(ZMQ)
+
+list(APPEND BEHAVIOR_TREE_PUBLIC_LIBRARIES
+    ${CMAKE_THREAD_LIBS_INIT}
+    ${CMAKE_DL_LIBS}
+)
+
+if( ZMQ_FOUND )
+    message(STATUS "ZeroMQ found.")
+    add_definitions( -DZMQ_FOUND )
+    list(APPEND BT_SOURCE src/loggers/bt_zmq_publisher.cpp)
+else()
+    message(WARNING "ZeroMQ NOT found. Skipping the build of [PublisherZMQ] and [bt_recorder].")
+endif()
+
+set(BEHAVIOR_TREE_LIBRARY ${PROJECT_NAME})
+
+# Update the policy setting to avoid an error when loading the ament_cmake package
+# at the current cmake version level
+if(POLICY CMP0057)
+    cmake_policy(SET CMP0057 NEW)
+endif()
+
+find_package(ament_cmake QUIET)
+
+if ( ament_cmake_FOUND )
+
+    # Not adding -DUSING_ROS since xml_parsing.cpp hasn't been ported to ROS2
+
+    message(STATUS "------------------------------------------")
+    message(STATUS "BehaviourTree is being built using AMENT.")
+    message(STATUS "------------------------------------------")
+
+    set(BUILD_TOOL_INCLUDE_DIRS ${ament_INCLUDE_DIRS})
+
+elseif( CATKIN_DEVEL_PREFIX OR CATKIN_BUILD_BINARY_PACKAGE)
+
+    set(catkin_FOUND 1)
+    add_definitions( -DUSING_ROS )
+    find_package(catkin REQUIRED COMPONENTS roslib)
+    find_package(GTest)
+
+    message(STATUS "------------------------------------------")
+    message(STATUS "BehaviourTree is being built using CATKIN.")
+    message(STATUS "------------------------------------------")
+
+    catkin_package(
+        INCLUDE_DIRS include # do not include "3rdparty" here
+        LIBRARIES ${BEHAVIOR_TREE_LIBRARY}
+        CATKIN_DEPENDS roslib
+        )
+
+    list(APPEND BEHAVIOR_TREE_PUBLIC_LIBRARIES ${catkin_LIBRARIES})
+    set(BUILD_TOOL_INCLUDE_DIRS ${catkin_INCLUDE_DIRS})
+
+elseif(BUILD_UNIT_TESTS)
+    find_package(GTest REQUIRED)
+endif()
+
+
+#############################################################
+if(ament_cmake_FOUND)
+    set( BEHAVIOR_TREE_LIB_DESTINATION   lib )
+    set( BEHAVIOR_TREE_INC_DESTINATION   include )
+    set( BEHAVIOR_TREE_BIN_DESTINATION   bin )
+
+    ament_export_include_directories(include)
+    ament_export_libraries(${BEHAVIOR_TREE_LIBRARY})
+    ament_package()
+elseif(catkin_FOUND)
+    set( BEHAVIOR_TREE_LIB_DESTINATION   ${CATKIN_PACKAGE_LIB_DESTINATION} )
+    set( BEHAVIOR_TREE_INC_DESTINATION   ${CATKIN_GLOBAL_INCLUDE_DESTINATION} )
+    set( BEHAVIOR_TREE_BIN_DESTINATION   ${CATKIN_GLOBAL_BIN_DESTINATION} )
+else()
+    set( BEHAVIOR_TREE_LIB_DESTINATION   lib )
+    set( BEHAVIOR_TREE_INC_DESTINATION   include )
+    set( BEHAVIOR_TREE_BIN_DESTINATION   bin )
+
+    set(CMAKE_RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${BEHAVIOR_TREE_BIN_DESTINATION}" )
+    set(CMAKE_LIBRARY_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${BEHAVIOR_TREE_LIB_DESTINATION}" )
+    set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${BEHAVIOR_TREE_BIN_DESTINATION}" )
+endif()
+
+message( STATUS "BEHAVIOR_TREE_LIB_DESTINATION:   ${BEHAVIOR_TREE_LIB_DESTINATION} " )
+message( STATUS "BEHAVIOR_TREE_BIN_DESTINATION:   ${BEHAVIOR_TREE_BIN_DESTINATION} " )
+message( STATUS "CMAKE_RUNTIME_OUTPUT_DIRECTORY:  ${CMAKE_RUNTIME_OUTPUT_DIRECTORY} " )
+message( STATUS "CMAKE_LIBRARY_OUTPUT_DIRECTORY:  ${CMAKE_LIBRARY_OUTPUT_DIRECTORY} " )
+message( STATUS "CMAKE_ARCHIVE_OUTPUT_DIRECTORY:  ${CMAKE_ARCHIVE_OUTPUT_DIRECTORY} " )
+
+#############################################################
+# LIBRARY
+
+list(APPEND BT_SOURCE
+    src/action_node.cpp
+    src/basic_types.cpp
+    src/behavior_tree.cpp
+    src/blackboard.cpp
+    src/bt_factory.cpp
+    src/decorator_node.cpp
+    src/condition_node.cpp
+    src/control_node.cpp
+    src/shared_library.cpp
+    src/tree_node.cpp
+    src/xml_parsing.cpp
+
+    src/decorators/inverter_node.cpp
+    src/decorators/repeat_node.cpp
+    src/decorators/retry_node.cpp
+    src/decorators/subtree_node.cpp
+    src/decorators/delay_node.cpp
+
+    src/controls/if_then_else_node.cpp
+    src/controls/fallback_node.cpp
+    src/controls/parallel_node.cpp
+    src/controls/reactive_sequence.cpp
+    src/controls/reactive_fallback.cpp
+    src/controls/sequence_node.cpp
+    src/controls/sequence_star_node.cpp
+    src/controls/switch_node.cpp
+    src/controls/while_do_else_node.cpp
+
+    src/loggers/bt_cout_logger.cpp
+    src/loggers/bt_file_logger.cpp
+    src/loggers/bt_minitrace_logger.cpp
+    src/private/tinyxml2.cpp
+
+    3rdparty/minitrace/minitrace.cpp
+    )
+
+find_package(Curses QUIET)
+
+if(CURSES_FOUND)
+    list(APPEND BT_SOURCE
+        src/controls/manual_node.cpp
+        )
+    list(APPEND BEHAVIOR_TREE_PUBLIC_LIBRARIES ${CURSES_LIBRARIES})
+    add_definitions(-DNCURSES_FOUND)
+endif()
+
+
+######################################################
+
+if (UNIX)
+    list(APPEND BT_SOURCE src/shared_library_UNIX.cpp )
+    if (BUILD_SHARED_LIBS)
+        add_library(${BEHAVIOR_TREE_LIBRARY} SHARED ${BT_SOURCE})
+    else()
+        add_library(${BEHAVIOR_TREE_LIBRARY} STATIC ${BT_SOURCE})
+    endif()
+endif()
+
+if (WIN32)
+    set(CMAKE_DEBUG_POSTFIX "d")
+    list(APPEND BT_SOURCE src/shared_library_WIN.cpp )
+    add_library(${BEHAVIOR_TREE_LIBRARY} STATIC ${BT_SOURCE} )
+endif()
+
+if( ZMQ_FOUND )
+    list(APPEND BUILD_TOOL_INCLUDE_DIRS ${ZMQ_INCLUDE_DIRS})
+endif()
+
+target_link_libraries(${BEHAVIOR_TREE_LIBRARY} PUBLIC
+    ${BEHAVIOR_TREE_PUBLIC_LIBRARIES})
+
+target_link_libraries(${BEHAVIOR_TREE_LIBRARY} PRIVATE
+    ${Boost_LIBRARIES}
+    ${ZMQ_LIBRARIES})
+
+#get_target_property(my_libs ${BEHAVIOR_TREE_LIBRARY} INTERFACE_LINK_LIBRARIES)
+#list(REMOVE_ITEM _libs X)
+#message("my_libs: ${my_libs}")
+
+#set_target_properties(${BEHAVIOR_TREE_LIBRARY} PROPERTIES INTERFACE_LINK_LIBRARIES "")
+
+target_compile_definitions(${BEHAVIOR_TREE_LIBRARY} PRIVATE $<$<CONFIG:Debug>:TINYXML2_DEBUG>)
+
+target_include_directories(${BEHAVIOR_TREE_LIBRARY} PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/3rdparty>
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>
+    ${BUILD_TOOL_INCLUDE_DIRS})
+
+if( ZMQ_FOUND )
+    target_compile_definitions(${BEHAVIOR_TREE_LIBRARY} PUBLIC ZMQ_FOUND)
+endif()
+
+if(MSVC)
+    target_compile_options(${BEHAVIOR_TREE_LIBRARY} PRIVATE /W3 /WX)
+else()
+    target_compile_options(${BEHAVIOR_TREE_LIBRARY} PRIVATE
+        -Wall -Wextra -Werror=return-type)
+endif()
+
+######################################################
+# Test
+if (BUILD_UNIT_TESTS)
+    add_subdirectory(tests)
+endif()
+
+######################################################
+# INSTALL
+
+INSTALL(TARGETS ${BEHAVIOR_TREE_LIBRARY}
+    EXPORT BehaviorTreeV3Config
+    ARCHIVE DESTINATION ${BEHAVIOR_TREE_LIB_DESTINATION}
+    LIBRARY DESTINATION ${BEHAVIOR_TREE_LIB_DESTINATION}
+    RUNTIME DESTINATION ${BEHAVIOR_TREE_BIN_DESTINATION}
+    )
+
+INSTALL( DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/include/
+    DESTINATION ${BEHAVIOR_TREE_INC_DESTINATION}
+    FILES_MATCHING PATTERN "*.h*")
+
+install(EXPORT BehaviorTreeV3Config
+    DESTINATION "${BEHAVIOR_TREE_LIB_DESTINATION}/BehaviorTreeV3/cmake"
+    NAMESPACE BT::)
+
+export(TARGETS ${PROJECT_NAME}
+    NAMESPACE BT::
+    FILE "${CMAKE_CURRENT_BINARY_DIR}/BehaviorTreeV3Config.cmake")
+
+export(PACKAGE ${PROJECT_NAME})
+
+######################################################
+# EXAMPLES and TOOLS
+if(BUILD_TOOLS)
+    add_subdirectory(tools)
+endif()
+
+if( BUILD_EXAMPLES )
+    add_subdirectory(sample_nodes)
+    add_subdirectory(examples)
+endif()
diff --git a/Doxyfile b/Doxyfile
new file mode 100644 (file)
index 0000000..8f51f6b
--- /dev/null
+++ b/Doxyfile
@@ -0,0 +1,2480 @@
+# Doxyfile 1.8.11
+
+# This file describes the settings to be used by the documentation system
+# doxygen (www.doxygen.org) for a project.
+#
+# All text after a double hash (##) is considered a comment and is placed in
+# front of the TAG it is preceding.
+#
+# All text after a single hash (#) is considered a comment and will be ignored.
+# The format is:
+# TAG = value [value, ...]
+# For lists, items can also be appended using:
+# TAG += value [value, ...]
+# Values that contain spaces should be placed between quotes (\" \").
+
+#---------------------------------------------------------------------------
+# Project related configuration options
+#---------------------------------------------------------------------------
+
+# This tag specifies the encoding used for all characters in the config file
+# that follow. The default is UTF-8 which is also the encoding used for all text
+# before the first occurrence of this tag. Doxygen uses libiconv (or the iconv
+# built into libc) for the transcoding. See http://www.gnu.org/software/libiconv
+# for the list of possible encodings.
+# The default value is: UTF-8.
+
+DOXYFILE_ENCODING      = UTF-8
+
+# The PROJECT_NAME tag is a single word (or a sequence of words surrounded by
+# double-quotes, unless you are using Doxywizard) that should identify the
+# project for which the documentation is generated. This name is used in the
+# title of most generated pages and in a few other places.
+# The default value is: My Project.
+
+PROJECT_NAME           = BehaviorTree
+
+# The PROJECT_NUMBER tag can be used to enter a project or revision number. This
+# could be handy for archiving the generated documentation or if some version
+# control system is used.
+
+PROJECT_NUMBER         = 
+
+# Using the PROJECT_BRIEF tag one can provide an optional one line description
+# for a project that appears at the top of each page and should give viewer a
+# quick idea about the purpose of the project. Keep the description short.
+
+PROJECT_BRIEF          = "Core Library to create and execute Behavior Trees"
+
+# With the PROJECT_LOGO tag one can specify a logo or an icon that is included
+# in the documentation. The maximum height of the logo should not exceed 55
+# pixels and the maximum width should not exceed 200 pixels. Doxygen will copy
+# the logo to the output directory.
+
+PROJECT_LOGO           = 
+
+# The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) path
+# into which the generated documentation will be written. If a relative path is
+# entered, it will be relative to the location where doxygen was started. If
+# left blank the current directory will be used.
+
+OUTPUT_DIRECTORY       = /home/davide.faconti/ws_behavior_tree/src/Behavior-Tree/doc
+
+# If the CREATE_SUBDIRS tag is set to YES then doxygen will create 4096 sub-
+# directories (in 2 levels) under the output directory of each output format and
+# will distribute the generated files over these directories. Enabling this
+# option can be useful when feeding doxygen a huge amount of source files, where
+# putting all generated files in the same directory would otherwise causes
+# performance problems for the file system.
+# The default value is: NO.
+
+CREATE_SUBDIRS         = YES
+
+# If the ALLOW_UNICODE_NAMES tag is set to YES, doxygen will allow non-ASCII
+# characters to appear in the names of generated files. If set to NO, non-ASCII
+# characters will be escaped, for example _xE3_x81_x84 will be used for Unicode
+# U+3044.
+# The default value is: NO.
+
+ALLOW_UNICODE_NAMES    = NO
+
+# The OUTPUT_LANGUAGE tag is used to specify the language in which all
+# documentation generated by doxygen is written. Doxygen will use this
+# information to generate all constant output in the proper language.
+# Possible values are: Afrikaans, Arabic, Armenian, Brazilian, Catalan, Chinese,
+# Chinese-Traditional, Croatian, Czech, Danish, Dutch, English (United States),
+# Esperanto, Farsi (Persian), Finnish, French, German, Greek, Hungarian,
+# Indonesian, Italian, Japanese, Japanese-en (Japanese with English messages),
+# Korean, Korean-en (Korean with English messages), Latvian, Lithuanian,
+# Macedonian, Norwegian, Persian (Farsi), Polish, Portuguese, Romanian, Russian,
+# Serbian, Serbian-Cyrillic, Slovak, Slovene, Spanish, Swedish, Turkish,
+# Ukrainian and Vietnamese.
+# The default value is: English.
+
+OUTPUT_LANGUAGE        = English
+
+# If the BRIEF_MEMBER_DESC tag is set to YES, doxygen will include brief member
+# descriptions after the members that are listed in the file and class
+# documentation (similar to Javadoc). Set to NO to disable this.
+# The default value is: YES.
+
+BRIEF_MEMBER_DESC      = YES
+
+# If the REPEAT_BRIEF tag is set to YES, doxygen will prepend the brief
+# description of a member or function before the detailed description
+#
+# Note: If both HIDE_UNDOC_MEMBERS and BRIEF_MEMBER_DESC are set to NO, the
+# brief descriptions will be completely suppressed.
+# The default value is: YES.
+
+REPEAT_BRIEF           = YES
+
+# This tag implements a quasi-intelligent brief description abbreviator that is
+# used to form the text in various listings. Each string in this list, if found
+# as the leading text of the brief description, will be stripped from the text
+# and the result, after processing the whole list, is used as the annotated
+# text. Otherwise, the brief description is used as-is. If left blank, the
+# following values are used ($name is automatically replaced with the name of
+# the entity):The $name class, The $name widget, The $name file, is, provides,
+# specifies, contains, represents, a, an and the.
+
+ABBREVIATE_BRIEF       = "The $name class" \
+                         "The $name widget" \
+                         "The $name file" \
+                         is \
+                         provides \
+                         specifies \
+                         contains \
+                         represents \
+                         a \
+                         an \
+                         the
+
+# If the ALWAYS_DETAILED_SEC and REPEAT_BRIEF tags are both set to YES then
+# doxygen will generate a detailed section even if there is only a brief
+# description.
+# The default value is: NO.
+
+ALWAYS_DETAILED_SEC    = NO
+
+# If the INLINE_INHERITED_MEMB tag is set to YES, doxygen will show all
+# inherited members of a class in the documentation of that class as if those
+# members were ordinary class members. Constructors, destructors and assignment
+# operators of the base classes will not be shown.
+# The default value is: NO.
+
+INLINE_INHERITED_MEMB  = NO
+
+# If the FULL_PATH_NAMES tag is set to YES, doxygen will prepend the full path
+# before files name in the file list and in the header files. If set to NO the
+# shortest path that makes the file name unique will be used
+# The default value is: YES.
+
+FULL_PATH_NAMES        = YES
+
+# The STRIP_FROM_PATH tag can be used to strip a user-defined part of the path.
+# Stripping is only done if one of the specified strings matches the left-hand
+# part of the path. The tag can be used to show relative paths in the file list.
+# If left blank the directory from which doxygen is run is used as the path to
+# strip.
+#
+# Note that you can specify absolute paths here, but also relative paths, which
+# will be relative from the directory where doxygen is started.
+# This tag requires that the tag FULL_PATH_NAMES is set to YES.
+
+STRIP_FROM_PATH        = 
+
+# The STRIP_FROM_INC_PATH tag can be used to strip a user-defined part of the
+# path mentioned in the documentation of a class, which tells the reader which
+# header file to include in order to use a class. If left blank only the name of
+# the header file containing the class definition is used. Otherwise one should
+# specify the list of include paths that are normally passed to the compiler
+# using the -I flag.
+
+STRIP_FROM_INC_PATH    = 
+
+# If the SHORT_NAMES tag is set to YES, doxygen will generate much shorter (but
+# less readable) file names. This can be useful is your file systems doesn't
+# support long names like on DOS, Mac, or CD-ROM.
+# The default value is: NO.
+
+SHORT_NAMES            = NO
+
+# If the JAVADOC_AUTOBRIEF tag is set to YES then doxygen will interpret the
+# first line (until the first dot) of a Javadoc-style comment as the brief
+# description. If set to NO, the Javadoc-style will behave just like regular Qt-
+# style comments (thus requiring an explicit @brief command for a brief
+# description.)
+# The default value is: NO.
+
+JAVADOC_AUTOBRIEF      = NO
+
+# If the QT_AUTOBRIEF tag is set to YES then doxygen will interpret the first
+# line (until the first dot) of a Qt-style comment as the brief description. If
+# set to NO, the Qt-style will behave just like regular Qt-style comments (thus
+# requiring an explicit \brief command for a brief description.)
+# The default value is: NO.
+
+QT_AUTOBRIEF           = NO
+
+# The MULTILINE_CPP_IS_BRIEF tag can be set to YES to make doxygen treat a
+# multi-line C++ special comment block (i.e. a block of //! or /// comments) as
+# a brief description. This used to be the default behavior. The new default is
+# to treat a multi-line C++ comment block as a detailed description. Set this
+# tag to YES if you prefer the old behavior instead.
+#
+# Note that setting this tag to YES also means that rational rose comments are
+# not recognized any more.
+# The default value is: NO.
+
+MULTILINE_CPP_IS_BRIEF = NO
+
+# If the INHERIT_DOCS tag is set to YES then an undocumented member inherits the
+# documentation from any documented member that it re-implements.
+# The default value is: YES.
+
+INHERIT_DOCS           = YES
+
+# If the SEPARATE_MEMBER_PAGES tag is set to YES then doxygen will produce a new
+# page for each member. If set to NO, the documentation of a member will be part
+# of the file/class/namespace that contains it.
+# The default value is: NO.
+
+SEPARATE_MEMBER_PAGES  = NO
+
+# The TAB_SIZE tag can be used to set the number of spaces in a tab. Doxygen
+# uses this value to replace tabs by spaces in code fragments.
+# Minimum value: 1, maximum value: 16, default value: 4.
+
+TAB_SIZE               = 4
+
+# This tag can be used to specify a number of aliases that act as commands in
+# the documentation. An alias has the form:
+# name=value
+# For example adding
+# "sideeffect=@par Side Effects:\n"
+# will allow you to put the command \sideeffect (or @sideeffect) in the
+# documentation, which will result in a user-defined paragraph with heading
+# "Side Effects:". You can put \n's in the value part of an alias to insert
+# newlines.
+
+ALIASES                = 
+
+# This tag can be used to specify a number of word-keyword mappings (TCL only).
+# A mapping has the form "name=value". For example adding "class=itcl::class"
+# will allow you to use the command class in the itcl::class meaning.
+
+TCL_SUBST              = 
+
+# Set the OPTIMIZE_OUTPUT_FOR_C tag to YES if your project consists of C sources
+# only. Doxygen will then generate output that is more tailored for C. For
+# instance, some of the names that are used will be different. The list of all
+# members will be omitted, etc.
+# The default value is: NO.
+
+OPTIMIZE_OUTPUT_FOR_C  = NO
+
+# Set the OPTIMIZE_OUTPUT_JAVA tag to YES if your project consists of Java or
+# Python sources only. Doxygen will then generate output that is more tailored
+# for that language. For instance, namespaces will be presented as packages,
+# qualified scopes will look different, etc.
+# The default value is: NO.
+
+OPTIMIZE_OUTPUT_JAVA   = NO
+
+# Set the OPTIMIZE_FOR_FORTRAN tag to YES if your project consists of Fortran
+# sources. Doxygen will then generate output that is tailored for Fortran.
+# The default value is: NO.
+
+OPTIMIZE_FOR_FORTRAN   = NO
+
+# Set the OPTIMIZE_OUTPUT_VHDL tag to YES if your project consists of VHDL
+# sources. Doxygen will then generate output that is tailored for VHDL.
+# The default value is: NO.
+
+OPTIMIZE_OUTPUT_VHDL   = NO
+
+# Doxygen selects the parser to use depending on the extension of the files it
+# parses. With this tag you can assign which parser to use for a given
+# extension. Doxygen has a built-in mapping, but you can override or extend it
+# using this tag. The format is ext=language, where ext is a file extension, and
+# language is one of the parsers supported by doxygen: IDL, Java, Javascript,
+# C#, C, C++, D, PHP, Objective-C, Python, Fortran (fixed format Fortran:
+# FortranFixed, free formatted Fortran: FortranFree, unknown formatted Fortran:
+# Fortran. In the later case the parser tries to guess whether the code is fixed
+# or free formatted code, this is the default for Fortran type files), VHDL. For
+# instance to make doxygen treat .inc files as Fortran files (default is PHP),
+# and .f files as C (default is Fortran), use: inc=Fortran f=C.
+#
+# Note: For files without extension you can use no_extension as a placeholder.
+#
+# Note that for custom extensions you also need to set FILE_PATTERNS otherwise
+# the files are not read by doxygen.
+
+EXTENSION_MAPPING      = 
+
+# If the MARKDOWN_SUPPORT tag is enabled then doxygen pre-processes all comments
+# according to the Markdown format, which allows for more readable
+# documentation. See http://daringfireball.net/projects/markdown/ for details.
+# The output of markdown processing is further processed by doxygen, so you can
+# mix doxygen, HTML, and XML commands with Markdown formatting. Disable only in
+# case of backward compatibilities issues.
+# The default value is: YES.
+
+MARKDOWN_SUPPORT       = YES
+
+# When enabled doxygen tries to link words that correspond to documented
+# classes, or namespaces to their corresponding documentation. Such a link can
+# be prevented in individual cases by putting a % sign in front of the word or
+# globally by setting AUTOLINK_SUPPORT to NO.
+# The default value is: YES.
+
+AUTOLINK_SUPPORT       = YES
+
+# If you use STL classes (i.e. std::string, std::vector, etc.) but do not want
+# to include (a tag file for) the STL sources as input, then you should set this
+# tag to YES in order to let doxygen match functions declarations and
+# definitions whose arguments contain STL classes (e.g. func(std::string);
+# versus func(std::string) {}). This also make the inheritance and collaboration
+# diagrams that involve STL classes more complete and accurate.
+# The default value is: NO.
+
+BUILTIN_STL_SUPPORT    = YES
+
+# If you use Microsoft's C++/CLI language, you should set this option to YES to
+# enable parsing support.
+# The default value is: NO.
+
+CPP_CLI_SUPPORT        = NO
+
+# Set the SIP_SUPPORT tag to YES if your project consists of sip (see:
+# http://www.riverbankcomputing.co.uk/software/sip/intro) sources only. Doxygen
+# will parse them like normal C++ but will assume all classes use public instead
+# of private inheritance when no explicit protection keyword is present.
+# The default value is: NO.
+
+SIP_SUPPORT            = NO
+
+# For Microsoft's IDL there are propget and propput attributes to indicate
+# getter and setter methods for a property. Setting this option to YES will make
+# doxygen to replace the get and set methods by a property in the documentation.
+# This will only work if the methods are indeed getting or setting a simple
+# type. If this is not the case, or you want to show the methods anyway, you
+# should set this option to NO.
+# The default value is: YES.
+
+IDL_PROPERTY_SUPPORT   = YES
+
+# If member grouping is used in the documentation and the DISTRIBUTE_GROUP_DOC
+# tag is set to YES then doxygen will reuse the documentation of the first
+# member in the group (if any) for the other members of the group. By default
+# all members of a group must be documented explicitly.
+# The default value is: NO.
+
+DISTRIBUTE_GROUP_DOC   = NO
+
+# If one adds a struct or class to a group and this option is enabled, then also
+# any nested class or struct is added to the same group. By default this option
+# is disabled and one has to add nested compounds explicitly via \ingroup.
+# The default value is: NO.
+
+GROUP_NESTED_COMPOUNDS = NO
+
+# Set the SUBGROUPING tag to YES to allow class member groups of the same type
+# (for instance a group of public functions) to be put as a subgroup of that
+# type (e.g. under the Public Functions section). Set it to NO to prevent
+# subgrouping. Alternatively, this can be done per class using the
+# \nosubgrouping command.
+# The default value is: YES.
+
+SUBGROUPING            = YES
+
+# When the INLINE_GROUPED_CLASSES tag is set to YES, classes, structs and unions
+# are shown inside the group in which they are included (e.g. using \ingroup)
+# instead of on a separate page (for HTML and Man pages) or section (for LaTeX
+# and RTF).
+#
+# Note that this feature does not work in combination with
+# SEPARATE_MEMBER_PAGES.
+# The default value is: NO.
+
+INLINE_GROUPED_CLASSES = NO
+
+# When the INLINE_SIMPLE_STRUCTS tag is set to YES, structs, classes, and unions
+# with only public data fields or simple typedef fields will be shown inline in
+# the documentation of the scope in which they are defined (i.e. file,
+# namespace, or group documentation), provided this scope is documented. If set
+# to NO, structs, classes, and unions are shown on a separate page (for HTML and
+# Man pages) or section (for LaTeX and RTF).
+# The default value is: NO.
+
+INLINE_SIMPLE_STRUCTS  = NO
+
+# When TYPEDEF_HIDES_STRUCT tag is enabled, a typedef of a struct, union, or
+# enum is documented as struct, union, or enum with the name of the typedef. So
+# typedef struct TypeS {} TypeT, will appear in the documentation as a struct
+# with name TypeT. When disabled the typedef will appear as a member of a file,
+# namespace, or class. And the struct will be named TypeS. This can typically be
+# useful for C code in case the coding convention dictates that all compound
+# types are typedef'ed and only the typedef is referenced, never the tag name.
+# The default value is: NO.
+
+TYPEDEF_HIDES_STRUCT   = NO
+
+# The size of the symbol lookup cache can be set using LOOKUP_CACHE_SIZE. This
+# cache is used to resolve symbols given their name and scope. Since this can be
+# an expensive process and often the same symbol appears multiple times in the
+# code, doxygen keeps a cache of pre-resolved symbols. If the cache is too small
+# doxygen will become slower. If the cache is too large, memory is wasted. The
+# cache size is given by this formula: 2^(16+LOOKUP_CACHE_SIZE). The valid range
+# is 0..9, the default is 0, corresponding to a cache size of 2^16=65536
+# symbols. At the end of a run doxygen will report the cache usage and suggest
+# the optimal cache size from a speed point of view.
+# Minimum value: 0, maximum value: 9, default value: 0.
+
+LOOKUP_CACHE_SIZE      = 0
+
+#---------------------------------------------------------------------------
+# Build related configuration options
+#---------------------------------------------------------------------------
+
+# If the EXTRACT_ALL tag is set to YES, doxygen will assume all entities in
+# documentation are documented, even if no documentation was available. Private
+# class members and static file members will be hidden unless the
+# EXTRACT_PRIVATE respectively EXTRACT_STATIC tags are set to YES.
+# Note: This will also disable the warnings about undocumented members that are
+# normally produced when WARNINGS is set to YES.
+# The default value is: NO.
+
+EXTRACT_ALL            = NO
+
+# If the EXTRACT_PRIVATE tag is set to YES, all private members of a class will
+# be included in the documentation.
+# The default value is: NO.
+
+EXTRACT_PRIVATE        = NO
+
+# If the EXTRACT_PACKAGE tag is set to YES, all members with package or internal
+# scope will be included in the documentation.
+# The default value is: NO.
+
+EXTRACT_PACKAGE        = NO
+
+# If the EXTRACT_STATIC tag is set to YES, all static members of a file will be
+# included in the documentation.
+# The default value is: NO.
+
+EXTRACT_STATIC         = NO
+
+# If the EXTRACT_LOCAL_CLASSES tag is set to YES, classes (and structs) defined
+# locally in source files will be included in the documentation. If set to NO,
+# only classes defined in header files are included. Does not have any effect
+# for Java sources.
+# The default value is: YES.
+
+EXTRACT_LOCAL_CLASSES  = YES
+
+# This flag is only useful for Objective-C code. If set to YES, local methods,
+# which are defined in the implementation section but not in the interface are
+# included in the documentation. If set to NO, only methods in the interface are
+# included.
+# The default value is: NO.
+
+EXTRACT_LOCAL_METHODS  = NO
+
+# If this flag is set to YES, the members of anonymous namespaces will be
+# extracted and appear in the documentation as a namespace called
+# 'anonymous_namespace{file}', where file will be replaced with the base name of
+# the file that contains the anonymous namespace. By default anonymous namespace
+# are hidden.
+# The default value is: NO.
+
+EXTRACT_ANON_NSPACES   = NO
+
+# If the HIDE_UNDOC_MEMBERS tag is set to YES, doxygen will hide all
+# undocumented members inside documented classes or files. If set to NO these
+# members will be included in the various overviews, but no documentation
+# section is generated. This option has no effect if EXTRACT_ALL is enabled.
+# The default value is: NO.
+
+HIDE_UNDOC_MEMBERS     = NO
+
+# If the HIDE_UNDOC_CLASSES tag is set to YES, doxygen will hide all
+# undocumented classes that are normally visible in the class hierarchy. If set
+# to NO, these classes will be included in the various overviews. This option
+# has no effect if EXTRACT_ALL is enabled.
+# The default value is: NO.
+
+HIDE_UNDOC_CLASSES     = NO
+
+# If the HIDE_FRIEND_COMPOUNDS tag is set to YES, doxygen will hide all friend
+# (class|struct|union) declarations. If set to NO, these declarations will be
+# included in the documentation.
+# The default value is: NO.
+
+HIDE_FRIEND_COMPOUNDS  = NO
+
+# If the HIDE_IN_BODY_DOCS tag is set to YES, doxygen will hide any
+# documentation blocks found inside the body of a function. If set to NO, these
+# blocks will be appended to the function's detailed documentation block.
+# The default value is: NO.
+
+HIDE_IN_BODY_DOCS      = NO
+
+# The INTERNAL_DOCS tag determines if documentation that is typed after a
+# \internal command is included. If the tag is set to NO then the documentation
+# will be excluded. Set it to YES to include the internal documentation.
+# The default value is: NO.
+
+INTERNAL_DOCS          = NO
+
+# If the CASE_SENSE_NAMES tag is set to NO then doxygen will only generate file
+# names in lower-case letters. If set to YES, upper-case letters are also
+# allowed. This is useful if you have classes or files whose names only differ
+# in case and if your file system supports case sensitive file names. Windows
+# and Mac users are advised to set this option to NO.
+# The default value is: system dependent.
+
+CASE_SENSE_NAMES       = NO
+
+# If the HIDE_SCOPE_NAMES tag is set to NO then doxygen will show members with
+# their full class and namespace scopes in the documentation. If set to YES, the
+# scope will be hidden.
+# The default value is: NO.
+
+HIDE_SCOPE_NAMES       = NO
+
+# If the HIDE_COMPOUND_REFERENCE tag is set to NO (default) then doxygen will
+# append additional text to a page's title, such as Class Reference. If set to
+# YES the compound reference will be hidden.
+# The default value is: NO.
+
+HIDE_COMPOUND_REFERENCE= NO
+
+# If the SHOW_INCLUDE_FILES tag is set to YES then doxygen will put a list of
+# the files that are included by a file in the documentation of that file.
+# The default value is: YES.
+
+SHOW_INCLUDE_FILES     = YES
+
+# If the SHOW_GROUPED_MEMB_INC tag is set to YES then Doxygen will add for each
+# grouped member an include statement to the documentation, telling the reader
+# which file to include in order to use the member.
+# The default value is: NO.
+
+SHOW_GROUPED_MEMB_INC  = NO
+
+# If the FORCE_LOCAL_INCLUDES tag is set to YES then doxygen will list include
+# files with double quotes in the documentation rather than with sharp brackets.
+# The default value is: NO.
+
+FORCE_LOCAL_INCLUDES   = NO
+
+# If the INLINE_INFO tag is set to YES then a tag [inline] is inserted in the
+# documentation for inline members.
+# The default value is: YES.
+
+INLINE_INFO            = YES
+
+# If the SORT_MEMBER_DOCS tag is set to YES then doxygen will sort the
+# (detailed) documentation of file and class members alphabetically by member
+# name. If set to NO, the members will appear in declaration order.
+# The default value is: YES.
+
+SORT_MEMBER_DOCS       = YES
+
+# If the SORT_BRIEF_DOCS tag is set to YES then doxygen will sort the brief
+# descriptions of file, namespace and class members alphabetically by member
+# name. If set to NO, the members will appear in declaration order. Note that
+# this will also influence the order of the classes in the class list.
+# The default value is: NO.
+
+SORT_BRIEF_DOCS        = NO
+
+# If the SORT_MEMBERS_CTORS_1ST tag is set to YES then doxygen will sort the
+# (brief and detailed) documentation of class members so that constructors and
+# destructors are listed first. If set to NO the constructors will appear in the
+# respective orders defined by SORT_BRIEF_DOCS and SORT_MEMBER_DOCS.
+# Note: If SORT_BRIEF_DOCS is set to NO this option is ignored for sorting brief
+# member documentation.
+# Note: If SORT_MEMBER_DOCS is set to NO this option is ignored for sorting
+# detailed member documentation.
+# The default value is: NO.
+
+SORT_MEMBERS_CTORS_1ST = NO
+
+# If the SORT_GROUP_NAMES tag is set to YES then doxygen will sort the hierarchy
+# of group names into alphabetical order. If set to NO the group names will
+# appear in their defined order.
+# The default value is: NO.
+
+SORT_GROUP_NAMES       = NO
+
+# If the SORT_BY_SCOPE_NAME tag is set to YES, the class list will be sorted by
+# fully-qualified names, including namespaces. If set to NO, the class list will
+# be sorted only by class name, not including the namespace part.
+# Note: This option is not very useful if HIDE_SCOPE_NAMES is set to YES.
+# Note: This option applies only to the class list, not to the alphabetical
+# list.
+# The default value is: NO.
+
+SORT_BY_SCOPE_NAME     = NO
+
+# If the STRICT_PROTO_MATCHING option is enabled and doxygen fails to do proper
+# type resolution of all parameters of a function it will reject a match between
+# the prototype and the implementation of a member function even if there is
+# only one candidate or it is obvious which candidate to choose by doing a
+# simple string match. By disabling STRICT_PROTO_MATCHING doxygen will still
+# accept a match between prototype and implementation in such cases.
+# The default value is: NO.
+
+STRICT_PROTO_MATCHING  = NO
+
+# The GENERATE_TODOLIST tag can be used to enable (YES) or disable (NO) the todo
+# list. This list is created by putting \todo commands in the documentation.
+# The default value is: YES.
+
+GENERATE_TODOLIST      = YES
+
+# The GENERATE_TESTLIST tag can be used to enable (YES) or disable (NO) the test
+# list. This list is created by putting \test commands in the documentation.
+# The default value is: YES.
+
+GENERATE_TESTLIST      = YES
+
+# The GENERATE_BUGLIST tag can be used to enable (YES) or disable (NO) the bug
+# list. This list is created by putting \bug commands in the documentation.
+# The default value is: YES.
+
+GENERATE_BUGLIST       = YES
+
+# The GENERATE_DEPRECATEDLIST tag can be used to enable (YES) or disable (NO)
+# the deprecated list. This list is created by putting \deprecated commands in
+# the documentation.
+# The default value is: YES.
+
+GENERATE_DEPRECATEDLIST= YES
+
+# The ENABLED_SECTIONS tag can be used to enable conditional documentation
+# sections, marked by \if <section_label> ... \endif and \cond <section_label>
+# ... \endcond blocks.
+
+ENABLED_SECTIONS       = 
+
+# The MAX_INITIALIZER_LINES tag determines the maximum number of lines that the
+# initial value of a variable or macro / define can have for it to appear in the
+# documentation. If the initializer consists of more lines than specified here
+# it will be hidden. Use a value of 0 to hide initializers completely. The
+# appearance of the value of individual variables and macros / defines can be
+# controlled using \showinitializer or \hideinitializer command in the
+# documentation regardless of this setting.
+# Minimum value: 0, maximum value: 10000, default value: 30.
+
+MAX_INITIALIZER_LINES  = 30
+
+# Set the SHOW_USED_FILES tag to NO to disable the list of files generated at
+# the bottom of the documentation of classes and structs. If set to YES, the
+# list will mention the files that were used to generate the documentation.
+# The default value is: YES.
+
+SHOW_USED_FILES        = YES
+
+# Set the SHOW_FILES tag to NO to disable the generation of the Files page. This
+# will remove the Files entry from the Quick Index and from the Folder Tree View
+# (if specified).
+# The default value is: YES.
+
+SHOW_FILES             = YES
+
+# Set the SHOW_NAMESPACES tag to NO to disable the generation of the Namespaces
+# page. This will remove the Namespaces entry from the Quick Index and from the
+# Folder Tree View (if specified).
+# The default value is: YES.
+
+SHOW_NAMESPACES        = YES
+
+# The FILE_VERSION_FILTER tag can be used to specify a program or script that
+# doxygen should invoke to get the current version for each file (typically from
+# the version control system). Doxygen will invoke the program by executing (via
+# popen()) the command command input-file, where command is the value of the
+# FILE_VERSION_FILTER tag, and input-file is the name of an input file provided
+# by doxygen. Whatever the program writes to standard output is used as the file
+# version. For an example see the documentation.
+
+FILE_VERSION_FILTER    = 
+
+# The LAYOUT_FILE tag can be used to specify a layout file which will be parsed
+# by doxygen. The layout file controls the global structure of the generated
+# output files in an output format independent way. To create the layout file
+# that represents doxygen's defaults, run doxygen with the -l option. You can
+# optionally specify a file name after the option, if omitted DoxygenLayout.xml
+# will be used as the name of the layout file.
+#
+# Note that if you run doxygen from a directory containing a file called
+# DoxygenLayout.xml, doxygen will parse it automatically even if the LAYOUT_FILE
+# tag is left empty.
+
+LAYOUT_FILE            = 
+
+# The CITE_BIB_FILES tag can be used to specify one or more bib files containing
+# the reference definitions. This must be a list of .bib files. The .bib
+# extension is automatically appended if omitted. This requires the bibtex tool
+# to be installed. See also http://en.wikipedia.org/wiki/BibTeX for more info.
+# For LaTeX the style of the bibliography can be controlled using
+# LATEX_BIB_STYLE. To use this feature you need bibtex and perl available in the
+# search path. See also \cite for info how to create references.
+
+CITE_BIB_FILES         = 
+
+#---------------------------------------------------------------------------
+# Configuration options related to warning and progress messages
+#---------------------------------------------------------------------------
+
+# The QUIET tag can be used to turn on/off the messages that are generated to
+# standard output by doxygen. If QUIET is set to YES this implies that the
+# messages are off.
+# The default value is: NO.
+
+QUIET                  = NO
+
+# The WARNINGS tag can be used to turn on/off the warning messages that are
+# generated to standard error (stderr) by doxygen. If WARNINGS is set to YES
+# this implies that the warnings are on.
+#
+# Tip: Turn warnings on while writing the documentation.
+# The default value is: YES.
+
+WARNINGS               = YES
+
+# If the WARN_IF_UNDOCUMENTED tag is set to YES then doxygen will generate
+# warnings for undocumented members. If EXTRACT_ALL is set to YES then this flag
+# will automatically be disabled.
+# The default value is: YES.
+
+WARN_IF_UNDOCUMENTED   = YES
+
+# If the WARN_IF_DOC_ERROR tag is set to YES, doxygen will generate warnings for
+# potential errors in the documentation, such as not documenting some parameters
+# in a documented function, or documenting parameters that don't exist or using
+# markup commands wrongly.
+# The default value is: YES.
+
+WARN_IF_DOC_ERROR      = YES
+
+# This WARN_NO_PARAMDOC option can be enabled to get warnings for functions that
+# are documented, but have no documentation for their parameters or return
+# value. If set to NO, doxygen will only warn about wrong or incomplete
+# parameter documentation, but not about the absence of documentation.
+# The default value is: NO.
+
+WARN_NO_PARAMDOC       = NO
+
+# If the WARN_AS_ERROR tag is set to YES then doxygen will immediately stop when
+# a warning is encountered.
+# The default value is: NO.
+
+WARN_AS_ERROR          = NO
+
+# The WARN_FORMAT tag determines the format of the warning messages that doxygen
+# can produce. The string should contain the $file, $line, and $text tags, which
+# will be replaced by the file and line number from which the warning originated
+# and the warning text. Optionally the format may contain $version, which will
+# be replaced by the version of the file (if it could be obtained via
+# FILE_VERSION_FILTER)
+# The default value is: $file:$line: $text.
+
+WARN_FORMAT            = "$file:$line: $text"
+
+# The WARN_LOGFILE tag can be used to specify a file to which warning and error
+# messages should be written. If left blank the output is written to standard
+# error (stderr).
+
+WARN_LOGFILE           = 
+
+#---------------------------------------------------------------------------
+# Configuration options related to the input files
+#---------------------------------------------------------------------------
+
+# The INPUT tag is used to specify the files and/or directories that contain
+# documented source files. You may enter file names like myfile.cpp or
+# directories like /usr/src/myproject. Separate the files or directories with
+# spaces. See also FILE_PATTERNS and EXTENSION_MAPPING
+# Note: If this tag is empty the current directory is searched.
+
+INPUT                  = /home/davide.faconti/ws_behavior_tree/src/Behavior-Tree/include
+
+# This tag can be used to specify the character encoding of the source files
+# that doxygen parses. Internally doxygen uses the UTF-8 encoding. Doxygen uses
+# libiconv (or the iconv built into libc) for the transcoding. See the libiconv
+# documentation (see: http://www.gnu.org/software/libiconv) for the list of
+# possible encodings.
+# The default value is: UTF-8.
+
+INPUT_ENCODING         = UTF-8
+
+# If the value of the INPUT tag contains directories, you can use the
+# FILE_PATTERNS tag to specify one or more wildcard patterns (like *.cpp and
+# *.h) to filter out the source-files in the directories.
+#
+# Note that for custom extensions or not directly supported extensions you also
+# need to set EXTENSION_MAPPING for the extension otherwise the files are not
+# read by doxygen.
+#
+# If left blank the following patterns are tested:*.c, *.cc, *.cxx, *.cpp,
+# *.c++, *.java, *.ii, *.ixx, *.ipp, *.i++, *.inl, *.idl, *.ddl, *.odl, *.h,
+# *.hh, *.hxx, *.hpp, *.h++, *.cs, *.d, *.php, *.php4, *.php5, *.phtml, *.inc,
+# *.m, *.markdown, *.md, *.mm, *.dox, *.py, *.pyw, *.f90, *.f, *.for, *.tcl,
+# *.vhd, *.vhdl, *.ucf, *.qsf, *.as and *.js.
+
+FILE_PATTERNS          = *.c \
+                         *.cc \
+                         *.cxx \
+                         *.cpp \
+                         *.c++ \
+                         *.java \
+                         *.ii \
+                         *.ixx \
+                         *.ipp \
+                         *.i++ \
+                         *.inl \
+                         *.idl \
+                         *.ddl \
+                         *.odl \
+                         *.h \
+                         *.hh \
+                         *.hxx \
+                         *.hpp \
+                         *.h++ \
+                         *.cs \
+                         *.d \
+                         *.php \
+                         *.php4 \
+                         *.php5 \
+                         *.phtml \
+                         *.inc \
+                         *.m \
+                         *.markdown \
+                         *.md \
+                         *.mm \
+                         *.dox \
+                         *.py \
+                         *.pyw \
+                         *.f90 \
+                         *.f \
+                         *.for \
+                         *.tcl \
+                         *.vhd \
+                         *.vhdl \
+                         *.ucf \
+                         *.qsf \
+                         *.as \
+                         *.js
+
+# The RECURSIVE tag can be used to specify whether or not subdirectories should
+# be searched for input files as well.
+# The default value is: NO.
+
+RECURSIVE              = YES
+
+# The EXCLUDE tag can be used to specify files and/or directories that should be
+# excluded from the INPUT source files. This way you can easily exclude a
+# subdirectory from a directory tree whose root is specified with the INPUT tag.
+#
+# Note that relative paths are relative to the directory from which doxygen is
+# run.
+
+EXCLUDE                = /home/davide.faconti/ws_behavior_tree/src/Behavior-Tree/3rdparty \
+                         /home/davide.faconti/ws_behavior_tree/src/Behavior-Tree/gtest
+
+# The EXCLUDE_SYMLINKS tag can be used to select whether or not files or
+# directories that are symbolic links (a Unix file system feature) are excluded
+# from the input.
+# The default value is: NO.
+
+EXCLUDE_SYMLINKS       = NO
+
+# If the value of the INPUT tag contains directories, you can use the
+# EXCLUDE_PATTERNS tag to specify one or more wildcard patterns to exclude
+# certain files from those directories.
+#
+# Note that the wildcards are matched against the file with absolute path, so to
+# exclude all test directories for example use the pattern */test/*
+
+EXCLUDE_PATTERNS       = 
+
+# The EXCLUDE_SYMBOLS tag can be used to specify one or more symbol names
+# (namespaces, classes, functions, etc.) that should be excluded from the
+# output. The symbol name can be a fully qualified name, a word, or if the
+# wildcard * is used, a substring. Examples: ANamespace, AClass,
+# AClass::ANamespace, ANamespace::*Test
+#
+# Note that the wildcards are matched against the file with absolute path, so to
+# exclude all test directories use the pattern */test/*
+
+EXCLUDE_SYMBOLS        = 
+
+# The EXAMPLE_PATH tag can be used to specify one or more files or directories
+# that contain example code fragments that are included (see the \include
+# command).
+
+EXAMPLE_PATH           = 
+
+# If the value of the EXAMPLE_PATH tag contains directories, you can use the
+# EXAMPLE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp and
+# *.h) to filter out the source-files in the directories. If left blank all
+# files are included.
+
+EXAMPLE_PATTERNS       = *
+
+# If the EXAMPLE_RECURSIVE tag is set to YES then subdirectories will be
+# searched for input files to be used with the \include or \dontinclude commands
+# irrespective of the value of the RECURSIVE tag.
+# The default value is: NO.
+
+EXAMPLE_RECURSIVE      = NO
+
+# The IMAGE_PATH tag can be used to specify one or more files or directories
+# that contain images that are to be included in the documentation (see the
+# \image command).
+
+IMAGE_PATH             = 
+
+# The INPUT_FILTER tag can be used to specify a program that doxygen should
+# invoke to filter for each input file. Doxygen will invoke the filter program
+# by executing (via popen()) the command:
+#
+# <filter> <input-file>
+#
+# where <filter> is the value of the INPUT_FILTER tag, and <input-file> is the
+# name of an input file. Doxygen will then use the output that the filter
+# program writes to standard output. If FILTER_PATTERNS is specified, this tag
+# will be ignored.
+#
+# Note that the filter must not add or remove lines; it is applied before the
+# code is scanned, but not when the output code is generated. If lines are added
+# or removed, the anchors will not be placed correctly.
+#
+# Note that for custom extensions or not directly supported extensions you also
+# need to set EXTENSION_MAPPING for the extension otherwise the files are not
+# properly processed by doxygen.
+
+INPUT_FILTER           = 
+
+# The FILTER_PATTERNS tag can be used to specify filters on a per file pattern
+# basis. Doxygen will compare the file name with each pattern and apply the
+# filter if there is a match. The filters are a list of the form: pattern=filter
+# (like *.cpp=my_cpp_filter). See INPUT_FILTER for further information on how
+# filters are used. If the FILTER_PATTERNS tag is empty or if none of the
+# patterns match the file name, INPUT_FILTER is applied.
+#
+# Note that for custom extensions or not directly supported extensions you also
+# need to set EXTENSION_MAPPING for the extension otherwise the files are not
+# properly processed by doxygen.
+
+FILTER_PATTERNS        = 
+
+# If the FILTER_SOURCE_FILES tag is set to YES, the input filter (if set using
+# INPUT_FILTER) will also be used to filter the input files that are used for
+# producing the source files to browse (i.e. when SOURCE_BROWSER is set to YES).
+# The default value is: NO.
+
+FILTER_SOURCE_FILES    = NO
+
+# The FILTER_SOURCE_PATTERNS tag can be used to specify source filters per file
+# pattern. A pattern will override the setting for FILTER_PATTERN (if any) and
+# it is also possible to disable source filtering for a specific pattern using
+# *.ext= (so without naming a filter).
+# This tag requires that the tag FILTER_SOURCE_FILES is set to YES.
+
+FILTER_SOURCE_PATTERNS = 
+
+# If the USE_MDFILE_AS_MAINPAGE tag refers to the name of a markdown file that
+# is part of the input, its contents will be placed on the main page
+# (index.html). This can be useful if you have a project on for instance GitHub
+# and want to reuse the introduction page also for the doxygen output.
+
+USE_MDFILE_AS_MAINPAGE = 
+
+#---------------------------------------------------------------------------
+# Configuration options related to source browsing
+#---------------------------------------------------------------------------
+
+# If the SOURCE_BROWSER tag is set to YES then a list of source files will be
+# generated. Documented entities will be cross-referenced with these sources.
+#
+# Note: To get rid of all source code in the generated output, make sure that
+# also VERBATIM_HEADERS is set to NO.
+# The default value is: NO.
+
+SOURCE_BROWSER         = NO
+
+# Setting the INLINE_SOURCES tag to YES will include the body of functions,
+# classes and enums directly into the documentation.
+# The default value is: NO.
+
+INLINE_SOURCES         = NO
+
+# Setting the STRIP_CODE_COMMENTS tag to YES will instruct doxygen to hide any
+# special comment blocks from generated source code fragments. Normal C, C++ and
+# Fortran comments will always remain visible.
+# The default value is: YES.
+
+STRIP_CODE_COMMENTS    = YES
+
+# If the REFERENCED_BY_RELATION tag is set to YES then for each documented
+# function all documented functions referencing it will be listed.
+# The default value is: NO.
+
+REFERENCED_BY_RELATION = NO
+
+# If the REFERENCES_RELATION tag is set to YES then for each documented function
+# all documented entities called/used by that function will be listed.
+# The default value is: NO.
+
+REFERENCES_RELATION    = NO
+
+# If the REFERENCES_LINK_SOURCE tag is set to YES and SOURCE_BROWSER tag is set
+# to YES then the hyperlinks from functions in REFERENCES_RELATION and
+# REFERENCED_BY_RELATION lists will link to the source code. Otherwise they will
+# link to the documentation.
+# The default value is: YES.
+
+REFERENCES_LINK_SOURCE = YES
+
+# If SOURCE_TOOLTIPS is enabled (the default) then hovering a hyperlink in the
+# source code will show a tooltip with additional information such as prototype,
+# brief description and links to the definition and documentation. Since this
+# will make the HTML file larger and loading of large files a bit slower, you
+# can opt to disable this feature.
+# The default value is: YES.
+# This tag requires that the tag SOURCE_BROWSER is set to YES.
+
+SOURCE_TOOLTIPS        = YES
+
+# If the USE_HTAGS tag is set to YES then the references to source code will
+# point to the HTML generated by the htags(1) tool instead of doxygen built-in
+# source browser. The htags tool is part of GNU's global source tagging system
+# (see http://www.gnu.org/software/global/global.html). You will need version
+# 4.8.6 or higher.
+#
+# To use it do the following:
+# - Install the latest version of global
+# - Enable SOURCE_BROWSER and USE_HTAGS in the config file
+# - Make sure the INPUT points to the root of the source tree
+# - Run doxygen as normal
+#
+# Doxygen will invoke htags (and that will in turn invoke gtags), so these
+# tools must be available from the command line (i.e. in the search path).
+#
+# The result: instead of the source browser generated by doxygen, the links to
+# source code will now point to the output of htags.
+# The default value is: NO.
+# This tag requires that the tag SOURCE_BROWSER is set to YES.
+
+USE_HTAGS              = NO
+
+# If the VERBATIM_HEADERS tag is set the YES then doxygen will generate a
+# verbatim copy of the header file for each class for which an include is
+# specified. Set to NO to disable this.
+# See also: Section \class.
+# The default value is: YES.
+
+VERBATIM_HEADERS       = YES
+
+# If the CLANG_ASSISTED_PARSING tag is set to YES then doxygen will use the
+# clang parser (see: http://clang.llvm.org/) for more accurate parsing at the
+# cost of reduced performance. This can be particularly helpful with template
+# rich C++ code for which doxygen's built-in parser lacks the necessary type
+# information.
+# Note: The availability of this option depends on whether or not doxygen was
+# generated with the -Duse-libclang=ON option for CMake.
+# The default value is: NO.
+
+CLANG_ASSISTED_PARSING = YES
+
+# If clang assisted parsing is enabled you can provide the compiler with command
+# line options that you would normally use when invoking the compiler. Note that
+# the include paths will already be set by doxygen for the files and directories
+# specified with INPUT and INCLUDE_PATH.
+# This tag requires that the tag CLANG_ASSISTED_PARSING is set to YES.
+
+CLANG_OPTIONS          = 
+
+#---------------------------------------------------------------------------
+# Configuration options related to the alphabetical class index
+#---------------------------------------------------------------------------
+
+# If the ALPHABETICAL_INDEX tag is set to YES, an alphabetical index of all
+# compounds will be generated. Enable this if the project contains a lot of
+# classes, structs, unions or interfaces.
+# The default value is: YES.
+
+ALPHABETICAL_INDEX     = YES
+
+# The COLS_IN_ALPHA_INDEX tag can be used to specify the number of columns in
+# which the alphabetical index list will be split.
+# Minimum value: 1, maximum value: 20, default value: 5.
+# This tag requires that the tag ALPHABETICAL_INDEX is set to YES.
+
+COLS_IN_ALPHA_INDEX    = 5
+
+# In case all classes in a project start with a common prefix, all classes will
+# be put under the same header in the alphabetical index. The IGNORE_PREFIX tag
+# can be used to specify a prefix (or a list of prefixes) that should be ignored
+# while generating the index headers.
+# This tag requires that the tag ALPHABETICAL_INDEX is set to YES.
+
+IGNORE_PREFIX          = 
+
+#---------------------------------------------------------------------------
+# Configuration options related to the HTML output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_HTML tag is set to YES, doxygen will generate HTML output
+# The default value is: YES.
+
+GENERATE_HTML          = YES
+
+# The HTML_OUTPUT tag is used to specify where the HTML docs will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it.
+# The default directory is: html.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_OUTPUT            = html
+
+# The HTML_FILE_EXTENSION tag can be used to specify the file extension for each
+# generated HTML page (for example: .htm, .php, .asp).
+# The default value is: .html.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_FILE_EXTENSION    = .html
+
+# The HTML_HEADER tag can be used to specify a user-defined HTML header file for
+# each generated HTML page. If the tag is left blank doxygen will generate a
+# standard header.
+#
+# To get valid HTML the header file that includes any scripts and style sheets
+# that doxygen needs, which is dependent on the configuration options used (e.g.
+# the setting GENERATE_TREEVIEW). It is highly recommended to start with a
+# default header using
+# doxygen -w html new_header.html new_footer.html new_stylesheet.css
+# YourConfigFile
+# and then modify the file new_header.html. See also section "Doxygen usage"
+# for information on how to generate the default header that doxygen normally
+# uses.
+# Note: The header is subject to change so you typically have to regenerate the
+# default header when upgrading to a newer version of doxygen. For a description
+# of the possible markers and block names see the documentation.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_HEADER            = 
+
+# The HTML_FOOTER tag can be used to specify a user-defined HTML footer for each
+# generated HTML page. If the tag is left blank doxygen will generate a standard
+# footer. See HTML_HEADER for more information on how to generate a default
+# footer and what special commands can be used inside the footer. See also
+# section "Doxygen usage" for information on how to generate the default footer
+# that doxygen normally uses.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_FOOTER            = 
+
+# The HTML_STYLESHEET tag can be used to specify a user-defined cascading style
+# sheet that is used by each HTML page. It can be used to fine-tune the look of
+# the HTML output. If left blank doxygen will generate a default style sheet.
+# See also section "Doxygen usage" for information on how to generate the style
+# sheet that doxygen normally uses.
+# Note: It is recommended to use HTML_EXTRA_STYLESHEET instead of this tag, as
+# it is more robust and this tag (HTML_STYLESHEET) will in the future become
+# obsolete.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_STYLESHEET        = 
+
+# The HTML_EXTRA_STYLESHEET tag can be used to specify additional user-defined
+# cascading style sheets that are included after the standard style sheets
+# created by doxygen. Using this option one can overrule certain style aspects.
+# This is preferred over using HTML_STYLESHEET since it does not replace the
+# standard style sheet and is therefore more robust against future updates.
+# Doxygen will copy the style sheet files to the output directory.
+# Note: The order of the extra style sheet files is of importance (e.g. the last
+# style sheet in the list overrules the setting of the previous ones in the
+# list). For an example see the documentation.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_EXTRA_STYLESHEET  = 
+
+# The HTML_EXTRA_FILES tag can be used to specify one or more extra images or
+# other source files which should be copied to the HTML output directory. Note
+# that these files will be copied to the base HTML output directory. Use the
+# $relpath^ marker in the HTML_HEADER and/or HTML_FOOTER files to load these
+# files. In the HTML_STYLESHEET file, use the file name only. Also note that the
+# files will be copied as-is; there are no commands or markers available.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_EXTRA_FILES       = 
+
+# The HTML_COLORSTYLE_HUE tag controls the color of the HTML output. Doxygen
+# will adjust the colors in the style sheet and background images according to
+# this color. Hue is specified as an angle on a colorwheel, see
+# http://en.wikipedia.org/wiki/Hue for more information. For instance the value
+# 0 represents red, 60 is yellow, 120 is green, 180 is cyan, 240 is blue, 300
+# purple, and 360 is red again.
+# Minimum value: 0, maximum value: 359, default value: 220.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_COLORSTYLE_HUE    = 220
+
+# The HTML_COLORSTYLE_SAT tag controls the purity (or saturation) of the colors
+# in the HTML output. For a value of 0 the output will use grayscales only. A
+# value of 255 will produce the most vivid colors.
+# Minimum value: 0, maximum value: 255, default value: 100.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_COLORSTYLE_SAT    = 100
+
+# The HTML_COLORSTYLE_GAMMA tag controls the gamma correction applied to the
+# luminance component of the colors in the HTML output. Values below 100
+# gradually make the output lighter, whereas values above 100 make the output
+# darker. The value divided by 100 is the actual gamma applied, so 80 represents
+# a gamma of 0.8, The value 220 represents a gamma of 2.2, and 100 does not
+# change the gamma.
+# Minimum value: 40, maximum value: 240, default value: 80.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_COLORSTYLE_GAMMA  = 80
+
+# If the HTML_TIMESTAMP tag is set to YES then the footer of each generated HTML
+# page will contain the date and time when the page was generated. Setting this
+# to YES can help to show when doxygen was last run and thus if the
+# documentation is up to date.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_TIMESTAMP         = NO
+
+# If the HTML_DYNAMIC_SECTIONS tag is set to YES then the generated HTML
+# documentation will contain sections that can be hidden and shown after the
+# page has loaded.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_DYNAMIC_SECTIONS  = NO
+
+# With HTML_INDEX_NUM_ENTRIES one can control the preferred number of entries
+# shown in the various tree structured indices initially; the user can expand
+# and collapse entries dynamically later on. Doxygen will expand the tree to
+# such a level that at most the specified number of entries are visible (unless
+# a fully collapsed tree already exceeds this amount). So setting the number of
+# entries 1 will produce a full collapsed tree by default. 0 is a special value
+# representing an infinite number of entries and will result in a full expanded
+# tree by default.
+# Minimum value: 0, maximum value: 9999, default value: 100.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_INDEX_NUM_ENTRIES = 100
+
+# If the GENERATE_DOCSET tag is set to YES, additional index files will be
+# generated that can be used as input for Apple's Xcode 3 integrated development
+# environment (see: http://developer.apple.com/tools/xcode/), introduced with
+# OSX 10.5 (Leopard). To create a documentation set, doxygen will generate a
+# Makefile in the HTML output directory. Running make will produce the docset in
+# that directory and running make install will install the docset in
+# ~/Library/Developer/Shared/Documentation/DocSets so that Xcode will find it at
+# startup. See http://developer.apple.com/tools/creatingdocsetswithdoxygen.html
+# for more information.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_DOCSET        = NO
+
+# This tag determines the name of the docset feed. A documentation feed provides
+# an umbrella under which multiple documentation sets from a single provider
+# (such as a company or product suite) can be grouped.
+# The default value is: Doxygen generated docs.
+# This tag requires that the tag GENERATE_DOCSET is set to YES.
+
+DOCSET_FEEDNAME        = "Doxygen generated docs"
+
+# This tag specifies a string that should uniquely identify the documentation
+# set bundle. This should be a reverse domain-name style string, e.g.
+# com.mycompany.MyDocSet. Doxygen will append .docset to the name.
+# The default value is: org.doxygen.Project.
+# This tag requires that the tag GENERATE_DOCSET is set to YES.
+
+DOCSET_BUNDLE_ID       = org.doxygen.Project
+
+# The DOCSET_PUBLISHER_ID tag specifies a string that should uniquely identify
+# the documentation publisher. This should be a reverse domain-name style
+# string, e.g. com.mycompany.MyDocSet.documentation.
+# The default value is: org.doxygen.Publisher.
+# This tag requires that the tag GENERATE_DOCSET is set to YES.
+
+DOCSET_PUBLISHER_ID    = org.doxygen.Publisher
+
+# The DOCSET_PUBLISHER_NAME tag identifies the documentation publisher.
+# The default value is: Publisher.
+# This tag requires that the tag GENERATE_DOCSET is set to YES.
+
+DOCSET_PUBLISHER_NAME  = Publisher
+
+# If the GENERATE_HTMLHELP tag is set to YES then doxygen generates three
+# additional HTML index files: index.hhp, index.hhc, and index.hhk. The
+# index.hhp is a project file that can be read by Microsoft's HTML Help Workshop
+# (see: http://www.microsoft.com/en-us/download/details.aspx?id=21138) on
+# Windows.
+#
+# The HTML Help Workshop contains a compiler that can convert all HTML output
+# generated by doxygen into a single compiled HTML file (.chm). Compiled HTML
+# files are now used as the Windows 98 help format, and will replace the old
+# Windows help format (.hlp) on all Windows platforms in the future. Compressed
+# HTML files also contain an index, a table of contents, and you can search for
+# words in the documentation. The HTML workshop also contains a viewer for
+# compressed HTML files.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_HTMLHELP      = NO
+
+# The CHM_FILE tag can be used to specify the file name of the resulting .chm
+# file. You can add a path in front of the file if the result should not be
+# written to the html output directory.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+CHM_FILE               = 
+
+# The HHC_LOCATION tag can be used to specify the location (absolute path
+# including file name) of the HTML help compiler (hhc.exe). If non-empty,
+# doxygen will try to run the HTML help compiler on the generated index.hhp.
+# The file has to be specified with full path.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+HHC_LOCATION           = 
+
+# The GENERATE_CHI flag controls if a separate .chi index file is generated
+# (YES) or that it should be included in the master .chm file (NO).
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+GENERATE_CHI           = NO
+
+# The CHM_INDEX_ENCODING is used to encode HtmlHelp index (hhk), content (hhc)
+# and project file content.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+CHM_INDEX_ENCODING     = 
+
+# The BINARY_TOC flag controls whether a binary table of contents is generated
+# (YES) or a normal table of contents (NO) in the .chm file. Furthermore it
+# enables the Previous and Next buttons.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+BINARY_TOC             = NO
+
+# The TOC_EXPAND flag can be set to YES to add extra items for group members to
+# the table of contents of the HTML help documentation and to the tree view.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+TOC_EXPAND             = NO
+
+# If the GENERATE_QHP tag is set to YES and both QHP_NAMESPACE and
+# QHP_VIRTUAL_FOLDER are set, an additional index file will be generated that
+# can be used as input for Qt's qhelpgenerator to generate a Qt Compressed Help
+# (.qch) of the generated HTML documentation.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_QHP           = NO
+
+# If the QHG_LOCATION tag is specified, the QCH_FILE tag can be used to specify
+# the file name of the resulting .qch file. The path specified is relative to
+# the HTML output folder.
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QCH_FILE               = 
+
+# The QHP_NAMESPACE tag specifies the namespace to use when generating Qt Help
+# Project output. For more information please see Qt Help Project / Namespace
+# (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#namespace).
+# The default value is: org.doxygen.Project.
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_NAMESPACE          = org.doxygen.Project
+
+# The QHP_VIRTUAL_FOLDER tag specifies the namespace to use when generating Qt
+# Help Project output. For more information please see Qt Help Project / Virtual
+# Folders (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#virtual-
+# folders).
+# The default value is: doc.
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_VIRTUAL_FOLDER     = doc
+
+# If the QHP_CUST_FILTER_NAME tag is set, it specifies the name of a custom
+# filter to add. For more information please see Qt Help Project / Custom
+# Filters (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#custom-
+# filters).
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_CUST_FILTER_NAME   = 
+
+# The QHP_CUST_FILTER_ATTRS tag specifies the list of the attributes of the
+# custom filter to add. For more information please see Qt Help Project / Custom
+# Filters (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#custom-
+# filters).
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_CUST_FILTER_ATTRS  = 
+
+# The QHP_SECT_FILTER_ATTRS tag specifies the list of the attributes this
+# project's filter section matches. Qt Help Project / Filter Attributes (see:
+# http://qt-project.org/doc/qt-4.8/qthelpproject.html#filter-attributes).
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_SECT_FILTER_ATTRS  = 
+
+# The QHG_LOCATION tag can be used to specify the location of Qt's
+# qhelpgenerator. If non-empty doxygen will try to run qhelpgenerator on the
+# generated .qhp file.
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHG_LOCATION           = 
+
+# If the GENERATE_ECLIPSEHELP tag is set to YES, additional index files will be
+# generated, together with the HTML files, they form an Eclipse help plugin. To
+# install this plugin and make it available under the help contents menu in
+# Eclipse, the contents of the directory containing the HTML and XML files needs
+# to be copied into the plugins directory of eclipse. The name of the directory
+# within the plugins directory should be the same as the ECLIPSE_DOC_ID value.
+# After copying Eclipse needs to be restarted before the help appears.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_ECLIPSEHELP   = NO
+
+# A unique identifier for the Eclipse help plugin. When installing the plugin
+# the directory name containing the HTML and XML files should also have this
+# name. Each documentation set should have its own identifier.
+# The default value is: org.doxygen.Project.
+# This tag requires that the tag GENERATE_ECLIPSEHELP is set to YES.
+
+ECLIPSE_DOC_ID         = org.doxygen.Project
+
+# If you want full control over the layout of the generated HTML pages it might
+# be necessary to disable the index and replace it with your own. The
+# DISABLE_INDEX tag can be used to turn on/off the condensed index (tabs) at top
+# of each HTML page. A value of NO enables the index and the value YES disables
+# it. Since the tabs in the index contain the same information as the navigation
+# tree, you can set this option to YES if you also set GENERATE_TREEVIEW to YES.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+DISABLE_INDEX          = NO
+
+# The GENERATE_TREEVIEW tag is used to specify whether a tree-like index
+# structure should be generated to display hierarchical information. If the tag
+# value is set to YES, a side panel will be generated containing a tree-like
+# index structure (just like the one that is generated for HTML Help). For this
+# to work a browser that supports JavaScript, DHTML, CSS and frames is required
+# (i.e. any modern browser). Windows users are probably better off using the
+# HTML help feature. Via custom style sheets (see HTML_EXTRA_STYLESHEET) one can
+# further fine-tune the look of the index. As an example, the default style
+# sheet generated by doxygen has an example that shows how to put an image at
+# the root of the tree instead of the PROJECT_NAME. Since the tree basically has
+# the same information as the tab index, you could consider setting
+# DISABLE_INDEX to YES when enabling this option.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_TREEVIEW      = YES
+
+# The ENUM_VALUES_PER_LINE tag can be used to set the number of enum values that
+# doxygen will group on one line in the generated HTML documentation.
+#
+# Note that a value of 0 will completely suppress the enum values from appearing
+# in the overview section.
+# Minimum value: 0, maximum value: 20, default value: 4.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+ENUM_VALUES_PER_LINE   = 4
+
+# If the treeview is enabled (see GENERATE_TREEVIEW) then this tag can be used
+# to set the initial width (in pixels) of the frame in which the tree is shown.
+# Minimum value: 0, maximum value: 1500, default value: 250.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+TREEVIEW_WIDTH         = 250
+
+# If the EXT_LINKS_IN_WINDOW option is set to YES, doxygen will open links to
+# external symbols imported via tag files in a separate window.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+EXT_LINKS_IN_WINDOW    = NO
+
+# Use this tag to change the font size of LaTeX formulas included as images in
+# the HTML documentation. When you change the font size after a successful
+# doxygen run you need to manually remove any form_*.png images from the HTML
+# output directory to force them to be regenerated.
+# Minimum value: 8, maximum value: 50, default value: 10.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+FORMULA_FONTSIZE       = 10
+
+# Use the FORMULA_TRANPARENT tag to determine whether or not the images
+# generated for formulas are transparent PNGs. Transparent PNGs are not
+# supported properly for IE 6.0, but are supported on all modern browsers.
+#
+# Note that when changing this option you need to delete any form_*.png files in
+# the HTML output directory before the changes have effect.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+FORMULA_TRANSPARENT    = YES
+
+# Enable the USE_MATHJAX option to render LaTeX formulas using MathJax (see
+# http://www.mathjax.org) which uses client side Javascript for the rendering
+# instead of using pre-rendered bitmaps. Use this if you do not have LaTeX
+# installed or if you want to formulas look prettier in the HTML output. When
+# enabled you may also need to install MathJax separately and configure the path
+# to it using the MATHJAX_RELPATH option.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+USE_MATHJAX            = NO
+
+# When MathJax is enabled you can set the default output format to be used for
+# the MathJax output. See the MathJax site (see:
+# http://docs.mathjax.org/en/latest/output.html) for more details.
+# Possible values are: HTML-CSS (which is slower, but has the best
+# compatibility), NativeMML (i.e. MathML) and SVG.
+# The default value is: HTML-CSS.
+# This tag requires that the tag USE_MATHJAX is set to YES.
+
+MATHJAX_FORMAT         = HTML-CSS
+
+# When MathJax is enabled you need to specify the location relative to the HTML
+# output directory using the MATHJAX_RELPATH option. The destination directory
+# should contain the MathJax.js script. For instance, if the mathjax directory
+# is located at the same level as the HTML output directory, then
+# MATHJAX_RELPATH should be ../mathjax. The default value points to the MathJax
+# Content Delivery Network so you can quickly see the result without installing
+# MathJax. However, it is strongly recommended to install a local copy of
+# MathJax from http://www.mathjax.org before deployment.
+# The default value is: http://cdn.mathjax.org/mathjax/latest.
+# This tag requires that the tag USE_MATHJAX is set to YES.
+
+MATHJAX_RELPATH        = http://cdn.mathjax.org/mathjax/latest
+
+# The MATHJAX_EXTENSIONS tag can be used to specify one or more MathJax
+# extension names that should be enabled during MathJax rendering. For example
+# MATHJAX_EXTENSIONS = TeX/AMSmath TeX/AMSsymbols
+# This tag requires that the tag USE_MATHJAX is set to YES.
+
+MATHJAX_EXTENSIONS     = 
+
+# The MATHJAX_CODEFILE tag can be used to specify a file with javascript pieces
+# of code that will be used on startup of the MathJax code. See the MathJax site
+# (see: http://docs.mathjax.org/en/latest/output.html) for more details. For an
+# example see the documentation.
+# This tag requires that the tag USE_MATHJAX is set to YES.
+
+MATHJAX_CODEFILE       = 
+
+# When the SEARCHENGINE tag is enabled doxygen will generate a search box for
+# the HTML output. The underlying search engine uses javascript and DHTML and
+# should work on any modern browser. Note that when using HTML help
+# (GENERATE_HTMLHELP), Qt help (GENERATE_QHP), or docsets (GENERATE_DOCSET)
+# there is already a search function so this one should typically be disabled.
+# For large projects the javascript based search engine can be slow, then
+# enabling SERVER_BASED_SEARCH may provide a better solution. It is possible to
+# search using the keyboard; to jump to the search box use <access key> + S
+# (what the <access key> is depends on the OS and browser, but it is typically
+# <CTRL>, <ALT>/<option>, or both). Inside the search box use the <cursor down
+# key> to jump into the search results window, the results can be navigated
+# using the <cursor keys>. Press <Enter> to select an item or <escape> to cancel
+# the search. The filter options can be selected when the cursor is inside the
+# search box by pressing <Shift>+<cursor down>. Also here use the <cursor keys>
+# to select a filter and <Enter> or <escape> to activate or cancel the filter
+# option.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+SEARCHENGINE           = YES
+
+# When the SERVER_BASED_SEARCH tag is enabled the search engine will be
+# implemented using a web server instead of a web client using Javascript. There
+# are two flavors of web server based searching depending on the EXTERNAL_SEARCH
+# setting. When disabled, doxygen will generate a PHP script for searching and
+# an index file used by the script. When EXTERNAL_SEARCH is enabled the indexing
+# and searching needs to be provided by external tools. See the section
+# "External Indexing and Searching" for details.
+# The default value is: NO.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+SERVER_BASED_SEARCH    = NO
+
+# When EXTERNAL_SEARCH tag is enabled doxygen will no longer generate the PHP
+# script for searching. Instead the search results are written to an XML file
+# which needs to be processed by an external indexer. Doxygen will invoke an
+# external search engine pointed to by the SEARCHENGINE_URL option to obtain the
+# search results.
+#
+# Doxygen ships with an example indexer (doxyindexer) and search engine
+# (doxysearch.cgi) which are based on the open source search engine library
+# Xapian (see: http://xapian.org/).
+#
+# See the section "External Indexing and Searching" for details.
+# The default value is: NO.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+EXTERNAL_SEARCH        = NO
+
+# The SEARCHENGINE_URL should point to a search engine hosted by a web server
+# which will return the search results when EXTERNAL_SEARCH is enabled.
+#
+# Doxygen ships with an example indexer (doxyindexer) and search engine
+# (doxysearch.cgi) which are based on the open source search engine library
+# Xapian (see: http://xapian.org/). See the section "External Indexing and
+# Searching" for details.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+SEARCHENGINE_URL       = 
+
+# When SERVER_BASED_SEARCH and EXTERNAL_SEARCH are both enabled the unindexed
+# search data is written to a file for indexing by an external tool. With the
+# SEARCHDATA_FILE tag the name of this file can be specified.
+# The default file is: searchdata.xml.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+SEARCHDATA_FILE        = searchdata.xml
+
+# When SERVER_BASED_SEARCH and EXTERNAL_SEARCH are both enabled the
+# EXTERNAL_SEARCH_ID tag can be used as an identifier for the project. This is
+# useful in combination with EXTRA_SEARCH_MAPPINGS to search through multiple
+# projects and redirect the results back to the right project.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+EXTERNAL_SEARCH_ID     = 
+
+# The EXTRA_SEARCH_MAPPINGS tag can be used to enable searching through doxygen
+# projects other than the one defined by this configuration file, but that are
+# all added to the same external search index. Each project needs to have a
+# unique id set via EXTERNAL_SEARCH_ID. The search mapping then maps the id of
+# to a relative location where the documentation can be found. The format is:
+# EXTRA_SEARCH_MAPPINGS = tagname1=loc1 tagname2=loc2 ...
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+EXTRA_SEARCH_MAPPINGS  = 
+
+#---------------------------------------------------------------------------
+# Configuration options related to the LaTeX output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_LATEX tag is set to YES, doxygen will generate LaTeX output.
+# The default value is: YES.
+
+GENERATE_LATEX         = NO
+
+# The LATEX_OUTPUT tag is used to specify where the LaTeX docs will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it.
+# The default directory is: latex.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_OUTPUT           = latex
+
+# The LATEX_CMD_NAME tag can be used to specify the LaTeX command name to be
+# invoked.
+#
+# Note that when enabling USE_PDFLATEX this option is only used for generating
+# bitmaps for formulas in the HTML output, but not in the Makefile that is
+# written to the output directory.
+# The default file is: latex.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_CMD_NAME         = latex
+
+# The MAKEINDEX_CMD_NAME tag can be used to specify the command name to generate
+# index for LaTeX.
+# The default file is: makeindex.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+MAKEINDEX_CMD_NAME     = makeindex
+
+# If the COMPACT_LATEX tag is set to YES, doxygen generates more compact LaTeX
+# documents. This may be useful for small projects and may help to save some
+# trees in general.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+COMPACT_LATEX          = NO
+
+# The PAPER_TYPE tag can be used to set the paper type that is used by the
+# printer.
+# Possible values are: a4 (210 x 297 mm), letter (8.5 x 11 inches), legal (8.5 x
+# 14 inches) and executive (7.25 x 10.5 inches).
+# The default value is: a4.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+PAPER_TYPE             = a4
+
+# The EXTRA_PACKAGES tag can be used to specify one or more LaTeX package names
+# that should be included in the LaTeX output. The package can be specified just
+# by its name or with the correct syntax as to be used with the LaTeX
+# \usepackage command. To get the times font for instance you can specify :
+# EXTRA_PACKAGES=times or EXTRA_PACKAGES={times}
+# To use the option intlimits with the amsmath package you can specify:
+# EXTRA_PACKAGES=[intlimits]{amsmath}
+# If left blank no extra packages will be included.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+EXTRA_PACKAGES         = 
+
+# The LATEX_HEADER tag can be used to specify a personal LaTeX header for the
+# generated LaTeX document. The header should contain everything until the first
+# chapter. If it is left blank doxygen will generate a standard header. See
+# section "Doxygen usage" for information on how to let doxygen write the
+# default header to a separate file.
+#
+# Note: Only use a user-defined header if you know what you are doing! The
+# following commands have a special meaning inside the header: $title,
+# $datetime, $date, $doxygenversion, $projectname, $projectnumber,
+# $projectbrief, $projectlogo. Doxygen will replace $title with the empty
+# string, for the replacement values of the other commands the user is referred
+# to HTML_HEADER.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_HEADER           = 
+
+# The LATEX_FOOTER tag can be used to specify a personal LaTeX footer for the
+# generated LaTeX document. The footer should contain everything after the last
+# chapter. If it is left blank doxygen will generate a standard footer. See
+# LATEX_HEADER for more information on how to generate a default footer and what
+# special commands can be used inside the footer.
+#
+# Note: Only use a user-defined footer if you know what you are doing!
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_FOOTER           = 
+
+# The LATEX_EXTRA_STYLESHEET tag can be used to specify additional user-defined
+# LaTeX style sheets that are included after the standard style sheets created
+# by doxygen. Using this option one can overrule certain style aspects. Doxygen
+# will copy the style sheet files to the output directory.
+# Note: The order of the extra style sheet files is of importance (e.g. the last
+# style sheet in the list overrules the setting of the previous ones in the
+# list).
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_EXTRA_STYLESHEET = 
+
+# The LATEX_EXTRA_FILES tag can be used to specify one or more extra images or
+# other source files which should be copied to the LATEX_OUTPUT output
+# directory. Note that the files will be copied as-is; there are no commands or
+# markers available.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_EXTRA_FILES      = 
+
+# If the PDF_HYPERLINKS tag is set to YES, the LaTeX that is generated is
+# prepared for conversion to PDF (using ps2pdf or pdflatex). The PDF file will
+# contain links (just like the HTML output) instead of page references. This
+# makes the output suitable for online browsing using a PDF viewer.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+PDF_HYPERLINKS         = YES
+
+# If the USE_PDFLATEX tag is set to YES, doxygen will use pdflatex to generate
+# the PDF file directly from the LaTeX files. Set this option to YES, to get a
+# higher quality PDF documentation.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+USE_PDFLATEX           = YES
+
+# If the LATEX_BATCHMODE tag is set to YES, doxygen will add the \batchmode
+# command to the generated LaTeX files. This will instruct LaTeX to keep running
+# if errors occur, instead of asking the user for help. This option is also used
+# when generating formulas in HTML.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_BATCHMODE        = NO
+
+# If the LATEX_HIDE_INDICES tag is set to YES then doxygen will not include the
+# index chapters (such as File Index, Compound Index, etc.) in the output.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_HIDE_INDICES     = NO
+
+# If the LATEX_SOURCE_CODE tag is set to YES then doxygen will include source
+# code with syntax highlighting in the LaTeX output.
+#
+# Note that which sources are shown also depends on other settings such as
+# SOURCE_BROWSER.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_SOURCE_CODE      = NO
+
+# The LATEX_BIB_STYLE tag can be used to specify the style to use for the
+# bibliography, e.g. plainnat, or ieeetr. See
+# http://en.wikipedia.org/wiki/BibTeX and \cite for more info.
+# The default value is: plain.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_BIB_STYLE        = plain
+
+# If the LATEX_TIMESTAMP tag is set to YES then the footer of each generated
+# page will contain the date and time when the page was generated. Setting this
+# to NO can help when comparing the output of multiple runs.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_TIMESTAMP        = NO
+
+#---------------------------------------------------------------------------
+# Configuration options related to the RTF output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_RTF tag is set to YES, doxygen will generate RTF output. The
+# RTF output is optimized for Word 97 and may not look too pretty with other RTF
+# readers/editors.
+# The default value is: NO.
+
+GENERATE_RTF           = NO
+
+# The RTF_OUTPUT tag is used to specify where the RTF docs will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it.
+# The default directory is: rtf.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_OUTPUT             = rtf
+
+# If the COMPACT_RTF tag is set to YES, doxygen generates more compact RTF
+# documents. This may be useful for small projects and may help to save some
+# trees in general.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+COMPACT_RTF            = NO
+
+# If the RTF_HYPERLINKS tag is set to YES, the RTF that is generated will
+# contain hyperlink fields. The RTF file will contain links (just like the HTML
+# output) instead of page references. This makes the output suitable for online
+# browsing using Word or some other Word compatible readers that support those
+# fields.
+#
+# Note: WordPad (write) and others do not support links.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_HYPERLINKS         = NO
+
+# Load stylesheet definitions from file. Syntax is similar to doxygen's config
+# file, i.e. a series of assignments. You only have to provide replacements,
+# missing definitions are set to their default value.
+#
+# See also section "Doxygen usage" for information on how to generate the
+# default style sheet that doxygen normally uses.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_STYLESHEET_FILE    = 
+
+# Set optional variables used in the generation of an RTF document. Syntax is
+# similar to doxygen's config file. A template extensions file can be generated
+# using doxygen -e rtf extensionFile.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_EXTENSIONS_FILE    = 
+
+# If the RTF_SOURCE_CODE tag is set to YES then doxygen will include source code
+# with syntax highlighting in the RTF output.
+#
+# Note that which sources are shown also depends on other settings such as
+# SOURCE_BROWSER.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_SOURCE_CODE        = NO
+
+#---------------------------------------------------------------------------
+# Configuration options related to the man page output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_MAN tag is set to YES, doxygen will generate man pages for
+# classes and files.
+# The default value is: NO.
+
+GENERATE_MAN           = NO
+
+# The MAN_OUTPUT tag is used to specify where the man pages will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it. A directory man3 will be created inside the directory specified by
+# MAN_OUTPUT.
+# The default directory is: man.
+# This tag requires that the tag GENERATE_MAN is set to YES.
+
+MAN_OUTPUT             = man
+
+# The MAN_EXTENSION tag determines the extension that is added to the generated
+# man pages. In case the manual section does not start with a number, the number
+# 3 is prepended. The dot (.) at the beginning of the MAN_EXTENSION tag is
+# optional.
+# The default value is: .3.
+# This tag requires that the tag GENERATE_MAN is set to YES.
+
+MAN_EXTENSION          = .3
+
+# The MAN_SUBDIR tag determines the name of the directory created within
+# MAN_OUTPUT in which the man pages are placed. If defaults to man followed by
+# MAN_EXTENSION with the initial . removed.
+# This tag requires that the tag GENERATE_MAN is set to YES.
+
+MAN_SUBDIR             = 
+
+# If the MAN_LINKS tag is set to YES and doxygen generates man output, then it
+# will generate one additional man file for each entity documented in the real
+# man page(s). These additional files only source the real man page, but without
+# them the man command would be unable to find the correct page.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_MAN is set to YES.
+
+MAN_LINKS              = NO
+
+#---------------------------------------------------------------------------
+# Configuration options related to the XML output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_XML tag is set to YES, doxygen will generate an XML file that
+# captures the structure of the code including all documentation.
+# The default value is: NO.
+
+GENERATE_XML           = NO
+
+# The XML_OUTPUT tag is used to specify where the XML pages will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it.
+# The default directory is: xml.
+# This tag requires that the tag GENERATE_XML is set to YES.
+
+XML_OUTPUT             = xml
+
+# If the XML_PROGRAMLISTING tag is set to YES, doxygen will dump the program
+# listings (including syntax highlighting and cross-referencing information) to
+# the XML output. Note that enabling this will significantly increase the size
+# of the XML output.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_XML is set to YES.
+
+XML_PROGRAMLISTING     = YES
+
+#---------------------------------------------------------------------------
+# Configuration options related to the DOCBOOK output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_DOCBOOK tag is set to YES, doxygen will generate Docbook files
+# that can be used to generate PDF.
+# The default value is: NO.
+
+GENERATE_DOCBOOK       = NO
+
+# The DOCBOOK_OUTPUT tag is used to specify where the Docbook pages will be put.
+# If a relative path is entered the value of OUTPUT_DIRECTORY will be put in
+# front of it.
+# The default directory is: docbook.
+# This tag requires that the tag GENERATE_DOCBOOK is set to YES.
+
+DOCBOOK_OUTPUT         = docbook
+
+# If the DOCBOOK_PROGRAMLISTING tag is set to YES, doxygen will include the
+# program listings (including syntax highlighting and cross-referencing
+# information) to the DOCBOOK output. Note that enabling this will significantly
+# increase the size of the DOCBOOK output.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_DOCBOOK is set to YES.
+
+DOCBOOK_PROGRAMLISTING = NO
+
+#---------------------------------------------------------------------------
+# Configuration options for the AutoGen Definitions output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_AUTOGEN_DEF tag is set to YES, doxygen will generate an
+# AutoGen Definitions (see http://autogen.sf.net) file that captures the
+# structure of the code including all documentation. Note that this feature is
+# still experimental and incomplete at the moment.
+# The default value is: NO.
+
+GENERATE_AUTOGEN_DEF   = NO
+
+#---------------------------------------------------------------------------
+# Configuration options related to the Perl module output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_PERLMOD tag is set to YES, doxygen will generate a Perl module
+# file that captures the structure of the code including all documentation.
+#
+# Note that this feature is still experimental and incomplete at the moment.
+# The default value is: NO.
+
+GENERATE_PERLMOD       = NO
+
+# If the PERLMOD_LATEX tag is set to YES, doxygen will generate the necessary
+# Makefile rules, Perl scripts and LaTeX code to be able to generate PDF and DVI
+# output from the Perl module output.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_PERLMOD is set to YES.
+
+PERLMOD_LATEX          = NO
+
+# If the PERLMOD_PRETTY tag is set to YES, the Perl module output will be nicely
+# formatted so it can be parsed by a human reader. This is useful if you want to
+# understand what is going on. On the other hand, if this tag is set to NO, the
+# size of the Perl module output will be much smaller and Perl will parse it
+# just the same.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_PERLMOD is set to YES.
+
+PERLMOD_PRETTY         = YES
+
+# The names of the make variables in the generated doxyrules.make file are
+# prefixed with the string contained in PERLMOD_MAKEVAR_PREFIX. This is useful
+# so different doxyrules.make files included by the same Makefile don't
+# overwrite each other's variables.
+# This tag requires that the tag GENERATE_PERLMOD is set to YES.
+
+PERLMOD_MAKEVAR_PREFIX = 
+
+#---------------------------------------------------------------------------
+# Configuration options related to the preprocessor
+#---------------------------------------------------------------------------
+
+# If the ENABLE_PREPROCESSING tag is set to YES, doxygen will evaluate all
+# C-preprocessor directives found in the sources and include files.
+# The default value is: YES.
+
+ENABLE_PREPROCESSING   = YES
+
+# If the MACRO_EXPANSION tag is set to YES, doxygen will expand all macro names
+# in the source code. If set to NO, only conditional compilation will be
+# performed. Macro expansion can be done in a controlled way by setting
+# EXPAND_ONLY_PREDEF to YES.
+# The default value is: NO.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+MACRO_EXPANSION        = NO
+
+# If the EXPAND_ONLY_PREDEF and MACRO_EXPANSION tags are both set to YES then
+# the macro expansion is limited to the macros specified with the PREDEFINED and
+# EXPAND_AS_DEFINED tags.
+# The default value is: NO.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+EXPAND_ONLY_PREDEF     = NO
+
+# If the SEARCH_INCLUDES tag is set to YES, the include files in the
+# INCLUDE_PATH will be searched if a #include is found.
+# The default value is: YES.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+SEARCH_INCLUDES        = YES
+
+# The INCLUDE_PATH tag can be used to specify one or more directories that
+# contain include files that are not input files but should be processed by the
+# preprocessor.
+# This tag requires that the tag SEARCH_INCLUDES is set to YES.
+
+INCLUDE_PATH           = 
+
+# You can use the INCLUDE_FILE_PATTERNS tag to specify one or more wildcard
+# patterns (like *.h and *.hpp) to filter out the header-files in the
+# directories. If left blank, the patterns specified with FILE_PATTERNS will be
+# used.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+INCLUDE_FILE_PATTERNS  = 
+
+# The PREDEFINED tag can be used to specify one or more macro names that are
+# defined before the preprocessor is started (similar to the -D option of e.g.
+# gcc). The argument of the tag is a list of macros of the form: name or
+# name=definition (no spaces). If the definition and the "=" are omitted, "=1"
+# is assumed. To prevent a macro definition from being undefined via #undef or
+# recursively expanded use the := operator instead of the = operator.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+PREDEFINED             = 
+
+# If the MACRO_EXPANSION and EXPAND_ONLY_PREDEF tags are set to YES then this
+# tag can be used to specify a list of macro names that should be expanded. The
+# macro definition that is found in the sources will be used. Use the PREDEFINED
+# tag if you want to use a different macro definition that overrules the
+# definition found in the source code.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+EXPAND_AS_DEFINED      = 
+
+# If the SKIP_FUNCTION_MACROS tag is set to YES then doxygen's preprocessor will
+# remove all references to function-like macros that are alone on a line, have
+# an all uppercase name, and do not end with a semicolon. Such function macros
+# are typically used for boiler-plate code, and will confuse the parser if not
+# removed.
+# The default value is: YES.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+SKIP_FUNCTION_MACROS   = YES
+
+#---------------------------------------------------------------------------
+# Configuration options related to external references
+#---------------------------------------------------------------------------
+
+# The TAGFILES tag can be used to specify one or more tag files. For each tag
+# file the location of the external documentation should be added. The format of
+# a tag file without this location is as follows:
+# TAGFILES = file1 file2 ...
+# Adding location for the tag files is done as follows:
+# TAGFILES = file1=loc1 "file2 = loc2" ...
+# where loc1 and loc2 can be relative or absolute paths or URLs. See the
+# section "Linking to external documentation" for more information about the use
+# of tag files.
+# Note: Each tag file must have a unique name (where the name does NOT include
+# the path). If a tag file is not located in the directory in which doxygen is
+# run, you must also specify the path to the tagfile here.
+
+TAGFILES               = 
+
+# When a file name is specified after GENERATE_TAGFILE, doxygen will create a
+# tag file that is based on the input files it reads. See section "Linking to
+# external documentation" for more information about the usage of tag files.
+
+GENERATE_TAGFILE       = 
+
+# If the ALLEXTERNALS tag is set to YES, all external class will be listed in
+# the class index. If set to NO, only the inherited external classes will be
+# listed.
+# The default value is: NO.
+
+ALLEXTERNALS           = NO
+
+# If the EXTERNAL_GROUPS tag is set to YES, all external groups will be listed
+# in the modules index. If set to NO, only the current project's groups will be
+# listed.
+# The default value is: YES.
+
+EXTERNAL_GROUPS        = YES
+
+# If the EXTERNAL_PAGES tag is set to YES, all external pages will be listed in
+# the related pages index. If set to NO, only the current project's pages will
+# be listed.
+# The default value is: YES.
+
+EXTERNAL_PAGES         = YES
+
+# The PERL_PATH should be the absolute path and name of the perl script
+# interpreter (i.e. the result of 'which perl').
+# The default file (with absolute path) is: /usr/bin/perl.
+
+PERL_PATH              = /usr/bin/perl
+
+#---------------------------------------------------------------------------
+# Configuration options related to the dot tool
+#---------------------------------------------------------------------------
+
+# If the CLASS_DIAGRAMS tag is set to YES, doxygen will generate a class diagram
+# (in HTML and LaTeX) for classes with base or super classes. Setting the tag to
+# NO turns the diagrams off. Note that this option also works with HAVE_DOT
+# disabled, but it is recommended to install and use dot, since it yields more
+# powerful graphs.
+# The default value is: YES.
+
+CLASS_DIAGRAMS         = YES
+
+# You can define message sequence charts within doxygen comments using the \msc
+# command. Doxygen will then run the mscgen tool (see:
+# http://www.mcternan.me.uk/mscgen/)) to produce the chart and insert it in the
+# documentation. The MSCGEN_PATH tag allows you to specify the directory where
+# the mscgen tool resides. If left empty the tool is assumed to be found in the
+# default search path.
+
+MSCGEN_PATH            = 
+
+# You can include diagrams made with dia in doxygen documentation. Doxygen will
+# then run dia to produce the diagram and insert it in the documentation. The
+# DIA_PATH tag allows you to specify the directory where the dia binary resides.
+# If left empty dia is assumed to be found in the default search path.
+
+DIA_PATH               = 
+
+# If set to YES the inheritance and collaboration graphs will hide inheritance
+# and usage relations if the target is undocumented or is not a class.
+# The default value is: YES.
+
+HIDE_UNDOC_RELATIONS   = YES
+
+# If you set the HAVE_DOT tag to YES then doxygen will assume the dot tool is
+# available from the path. This tool is part of Graphviz (see:
+# http://www.graphviz.org/), a graph visualization toolkit from AT&T and Lucent
+# Bell Labs. The other options in this section have no effect if this option is
+# set to NO
+# The default value is: YES.
+
+HAVE_DOT               = YES
+
+# The DOT_NUM_THREADS specifies the number of dot invocations doxygen is allowed
+# to run in parallel. When set to 0 doxygen will base this on the number of
+# processors available in the system. You can set it explicitly to a value
+# larger than 0 to get control over the balance between CPU load and processing
+# speed.
+# Minimum value: 0, maximum value: 32, default value: 0.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_NUM_THREADS        = 0
+
+# When you want a differently looking font in the dot files that doxygen
+# generates you can specify the font name using DOT_FONTNAME. You need to make
+# sure dot is able to find the font, which can be done by putting it in a
+# standard location or by setting the DOTFONTPATH environment variable or by
+# setting DOT_FONTPATH to the directory containing the font.
+# The default value is: Helvetica.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_FONTNAME           = Helvetica
+
+# The DOT_FONTSIZE tag can be used to set the size (in points) of the font of
+# dot graphs.
+# Minimum value: 4, maximum value: 24, default value: 10.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_FONTSIZE           = 10
+
+# By default doxygen will tell dot to use the default font as specified with
+# DOT_FONTNAME. If you specify a different font using DOT_FONTNAME you can set
+# the path where dot can find it using this tag.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_FONTPATH           = 
+
+# If the CLASS_GRAPH tag is set to YES then doxygen will generate a graph for
+# each documented class showing the direct and indirect inheritance relations.
+# Setting this tag to YES will force the CLASS_DIAGRAMS tag to NO.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+CLASS_GRAPH            = YES
+
+# If the COLLABORATION_GRAPH tag is set to YES then doxygen will generate a
+# graph for each documented class showing the direct and indirect implementation
+# dependencies (inheritance, containment, and class references variables) of the
+# class with other documented classes.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+COLLABORATION_GRAPH    = YES
+
+# If the GROUP_GRAPHS tag is set to YES then doxygen will generate a graph for
+# groups, showing the direct groups dependencies.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+GROUP_GRAPHS           = YES
+
+# If the UML_LOOK tag is set to YES, doxygen will generate inheritance and
+# collaboration diagrams in a style similar to the OMG's Unified Modeling
+# Language.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+UML_LOOK               = NO
+
+# If the UML_LOOK tag is enabled, the fields and methods are shown inside the
+# class node. If there are many fields or methods and many nodes the graph may
+# become too big to be useful. The UML_LIMIT_NUM_FIELDS threshold limits the
+# number of items for each type to make the size more manageable. Set this to 0
+# for no limit. Note that the threshold may be exceeded by 50% before the limit
+# is enforced. So when you set the threshold to 10, up to 15 fields may appear,
+# but if the number exceeds 15, the total amount of fields shown is limited to
+# 10.
+# Minimum value: 0, maximum value: 100, default value: 10.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+UML_LIMIT_NUM_FIELDS   = 10
+
+# If the TEMPLATE_RELATIONS tag is set to YES then the inheritance and
+# collaboration graphs will show the relations between templates and their
+# instances.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+TEMPLATE_RELATIONS     = NO
+
+# If the INCLUDE_GRAPH, ENABLE_PREPROCESSING and SEARCH_INCLUDES tags are set to
+# YES then doxygen will generate a graph for each documented file showing the
+# direct and indirect include dependencies of the file with other documented
+# files.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+INCLUDE_GRAPH          = YES
+
+# If the INCLUDED_BY_GRAPH, ENABLE_PREPROCESSING and SEARCH_INCLUDES tags are
+# set to YES then doxygen will generate a graph for each documented file showing
+# the direct and indirect include dependencies of the file with other documented
+# files.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+INCLUDED_BY_GRAPH      = YES
+
+# If the CALL_GRAPH tag is set to YES then doxygen will generate a call
+# dependency graph for every global function or class method.
+#
+# Note that enabling this option will significantly increase the time of a run.
+# So in most cases it will be better to enable call graphs for selected
+# functions only using the \callgraph command. Disabling a call graph can be
+# accomplished by means of the command \hidecallgraph.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+CALL_GRAPH             = NO
+
+# If the CALLER_GRAPH tag is set to YES then doxygen will generate a caller
+# dependency graph for every global function or class method.
+#
+# Note that enabling this option will significantly increase the time of a run.
+# So in most cases it will be better to enable caller graphs for selected
+# functions only using the \callergraph command. Disabling a caller graph can be
+# accomplished by means of the command \hidecallergraph.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+CALLER_GRAPH           = NO
+
+# If the GRAPHICAL_HIERARCHY tag is set to YES then doxygen will graphical
+# hierarchy of all classes instead of a textual one.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+GRAPHICAL_HIERARCHY    = YES
+
+# If the DIRECTORY_GRAPH tag is set to YES then doxygen will show the
+# dependencies a directory has on other directories in a graphical way. The
+# dependency relations are determined by the #include relations between the
+# files in the directories.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DIRECTORY_GRAPH        = YES
+
+# The DOT_IMAGE_FORMAT tag can be used to set the image format of the images
+# generated by dot. For an explanation of the image formats see the section
+# output formats in the documentation of the dot tool (Graphviz (see:
+# http://www.graphviz.org/)).
+# Note: If you choose svg you need to set HTML_FILE_EXTENSION to xhtml in order
+# to make the SVG files visible in IE 9+ (other browsers do not have this
+# requirement).
+# Possible values are: png, png:cairo, png:cairo:cairo, png:cairo:gd, png:gd,
+# png:gd:gd, jpg, jpg:cairo, jpg:cairo:gd, jpg:gd, jpg:gd:gd, gif, gif:cairo,
+# gif:cairo:gd, gif:gd, gif:gd:gd, svg, png:gd, png:gd:gd, png:cairo,
+# png:cairo:gd, png:cairo:cairo, png:cairo:gdiplus, png:gdiplus and
+# png:gdiplus:gdiplus.
+# The default value is: png.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_IMAGE_FORMAT       = png
+
+# If DOT_IMAGE_FORMAT is set to svg, then this option can be set to YES to
+# enable generation of interactive SVG images that allow zooming and panning.
+#
+# Note that this requires a modern browser other than Internet Explorer. Tested
+# and working are Firefox, Chrome, Safari, and Opera.
+# Note: For IE 9+ you need to set HTML_FILE_EXTENSION to xhtml in order to make
+# the SVG files visible. Older versions of IE do not have SVG support.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+INTERACTIVE_SVG        = NO
+
+# The DOT_PATH tag can be used to specify the path where the dot tool can be
+# found. If left blank, it is assumed the dot tool can be found in the path.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_PATH               = 
+
+# The DOTFILE_DIRS tag can be used to specify one or more directories that
+# contain dot files that are included in the documentation (see the \dotfile
+# command).
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOTFILE_DIRS           = 
+
+# The MSCFILE_DIRS tag can be used to specify one or more directories that
+# contain msc files that are included in the documentation (see the \mscfile
+# command).
+
+MSCFILE_DIRS           = 
+
+# The DIAFILE_DIRS tag can be used to specify one or more directories that
+# contain dia files that are included in the documentation (see the \diafile
+# command).
+
+DIAFILE_DIRS           = 
+
+# When using plantuml, the PLANTUML_JAR_PATH tag should be used to specify the
+# path where java can find the plantuml.jar file. If left blank, it is assumed
+# PlantUML is not used or called during a preprocessing step. Doxygen will
+# generate a warning when it encounters a \startuml command in this case and
+# will not generate output for the diagram.
+
+PLANTUML_JAR_PATH      = 
+
+# When using plantuml, the specified paths are searched for files specified by
+# the !include statement in a plantuml block.
+
+PLANTUML_INCLUDE_PATH  = 
+
+# The DOT_GRAPH_MAX_NODES tag can be used to set the maximum number of nodes
+# that will be shown in the graph. If the number of nodes in a graph becomes
+# larger than this value, doxygen will truncate the graph, which is visualized
+# by representing a node as a red box. Note that doxygen if the number of direct
+# children of the root node in a graph is already larger than
+# DOT_GRAPH_MAX_NODES then the graph will not be shown at all. Also note that
+# the size of a graph can be further restricted by MAX_DOT_GRAPH_DEPTH.
+# Minimum value: 0, maximum value: 10000, default value: 50.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_GRAPH_MAX_NODES    = 50
+
+# The MAX_DOT_GRAPH_DEPTH tag can be used to set the maximum depth of the graphs
+# generated by dot. A depth value of 3 means that only nodes reachable from the
+# root by following a path via at most 3 edges will be shown. Nodes that lay
+# further from the root node will be omitted. Note that setting this option to 1
+# or 2 may greatly reduce the computation time needed for large code bases. Also
+# note that the size of a graph can be further restricted by
+# DOT_GRAPH_MAX_NODES. Using a depth of 0 means no depth restriction.
+# Minimum value: 0, maximum value: 1000, default value: 0.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+MAX_DOT_GRAPH_DEPTH    = 0
+
+# Set the DOT_TRANSPARENT tag to YES to generate images with a transparent
+# background. This is disabled by default, because dot on Windows does not seem
+# to support this out of the box.
+#
+# Warning: Depending on the platform used, enabling this option may lead to
+# badly anti-aliased labels on the edges of a graph (i.e. they become hard to
+# read).
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_TRANSPARENT        = NO
+
+# Set the DOT_MULTI_TARGETS tag to YES to allow dot to generate multiple output
+# files in one run (i.e. multiple -o and -T options on the command line). This
+# makes dot run faster, but since only newer versions of dot (>1.8.10) support
+# this, this feature is disabled by default.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_MULTI_TARGETS      = NO
+
+# If the GENERATE_LEGEND tag is set to YES doxygen will generate a legend page
+# explaining the meaning of the various boxes and arrows in the dot generated
+# graphs.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+GENERATE_LEGEND        = YES
+
+# If the DOT_CLEANUP tag is set to YES, doxygen will remove the intermediate dot
+# files that are used to generate the various graphs.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_CLEANUP            = YES
diff --git a/LICENSE b/LICENSE
new file mode 100644 (file)
index 0000000..f60806f
--- /dev/null
+++ b/LICENSE
@@ -0,0 +1,22 @@
+The MIT License (MIT)
+
+Copyright (c) 2015 - 2018 Michele Colledanchise
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+
diff --git a/MOOD2Be_final_report.pdf b/MOOD2Be_final_report.pdf
new file mode 100644 (file)
index 0000000..ff77225
Binary files /dev/null and b/MOOD2Be_final_report.pdf differ
diff --git a/README.md b/README.md
new file mode 100644 (file)
index 0000000..968893a
--- /dev/null
+++ b/README.md
@@ -0,0 +1,179 @@
+![License MIT](https://img.shields.io/github/license/BehaviorTree/BehaviorTree.CPP?color=blue)
+![Version](https://img.shields.io/badge/version-3.5-blue.svg)
+[![Build Status](https://travis-ci.org/BehaviorTree/BehaviorTree.CPP.svg?branch=master)](https://travis-ci.org/BehaviorTree/BehaviorTree.CPP)
+[![ros1](https://github.com/BehaviorTree/BehaviorTree.CPP/workflows/ros1/badge.svg?branch=master)](https://github.com/BehaviorTree/BehaviorTree.CPP/actions?query=workflow%3Aros1)
+[![ros2](https://github.com/BehaviorTree/BehaviorTree.CPP/workflows/ros2/badge.svg?branch=master)](https://github.com/BehaviorTree/BehaviorTree.CPP/actions?query=workflow%3Aros2)
+[![Build status](https://ci.appveyor.com/api/projects/status/8lawroklgnrkg38f?svg=true)](https://ci.appveyor.com/project/facontidavide59577/behaviortree-cpp)
+[![Codacy Badge](https://app.codacy.com/project/badge/Grade/f7489a1758ab47d49f62342f9649b62a)](https://www.codacy.com/manual/davide.faconti/BehaviorTree.CPP?utm_source=github.com&amp;utm_medium=referral&amp;utm_content=BehaviorTree/BehaviorTree.CPP&amp;utm_campaign=Badge_Grade)
+[![LGTM Grade](https://img.shields.io/lgtm/grade/cpp/github/BehaviorTree/BehaviorTree.CPP)](https://lgtm.com/projects/g/BehaviorTree/BehaviorTree.CPP/context:cpp)
+[![Join the chat at https://gitter.im/BehaviorTree-ROS/Lobby](https://badges.gitter.im/Join%20Chat.svg)](https://gitter.im/BehaviorTree-ROS/Lobby?utm_source=badge&utm_medium=badge&utm_content=badge)
+
+# BehaviorTree.CPP
+
+<p align="center"><img src="docs/images/ReadTheDocs.png"></p>
+
+This  __C++ 14__ library provides a framework to create BehaviorTrees.
+It was designed to be flexible, easy to use, reactive and fast.
+
+Even if our main use-case is __robotics__, you can use this library to build
+__AI for games__, or to replace Finite State Machines in your application.
+
+There are few features that make __BehaviorTree.CPP__ unique, when compared to other implementations:
+
+- It makes __asynchronous Actions__, i.e. non-blocking, a first-class citizen.
+
+- You can build __reactive__ behaviors that execute multiple Actions concurrently.
+
+- Trees are defined using a Domain Specific Scripting __scripting language__ (based on XML), and can be loaded at run-time; in other words, even if written in C++, Trees are _not_ hard-coded.
+
+- You can statically link your custom TreeNodes or convert them into __plugins__
+which can be loaded at run-time.
+
+- It provides a type-safe and flexible mechanism to do __Dataflow__ between
+  Nodes of the Tree.
+
+- It includes a __logging/profiling__ infrastructure that allows the user 
+to visualize, record, replay and analyze state transitions.
+
+- Last but not least: it is well [documented](https://www.behaviortree.dev/)!
+
+# Documentation
+
+You can learn about the main concepts, the API and the tutorials here: https://www.behaviortree.dev/
+
+To find more details about the conceptual ideas that make this implementation different from others, you can read the [final deliverable of the project MOOD2Be](https://github.com/BehaviorTree/BehaviorTree.CPP/blob/master/MOOD2Be_final_report.pdf).
+
+# Does your company use BehaviorTree.CPP?
+
+No company, institution or public/private funding is currently supporting the development of BehaviorTree.CPP and Groot.
+
+Consider becoming a **sponsor** to support bug fixing and development of new features. You can find contact details in [package.xml](package.xml).
+
+# Design principles
+
+The main goal of this project is to create a Behavior Tree implementation
+that uses the principles of Model Driven Development to separate the role 
+of the __Component Developer__ from the __Behavior Designer__.
+
+In practice, this means that:
+
+- Custom TreeNodes must be reusable building blocks. 
+ You should be able to implement them once and reuse them to build many behaviors.
+
+- To build a Behavior Tree out of TreeNodes, the Behavior Designer must 
+not need to read nor modify the C++ source code of a given TreeNode.
+
+- Complex Behaviours must be composable using Subtrees
+
+Many of the features and, sometimes, the apparent limitations of this library, might be a consequence 
+of this design principle. 
+
+For instance, having a scoped BlackBoard, visible only in a portion of the tree, is particularly important 
+to avoid "name pollution" and allow the creation of large scale trees.
+
+# GUI Editor
+
+Editing a BehaviorTree is as simple as editing a XML file in your favourite text editor.
+
+If you are looking for a more fancy graphical user interface (and I know you do) check 
+[Groot](https://github.com/BehaviorTree/Groot) out.
+
+![Groot screenshot](docs/groot-screenshot.png)
+
+## Watch Groot and BehaviorTree.CPP in action
+
+Click on the following image to see a short video of how the C++ library and
+the graphic user interface are used to design and monitor a Behavior Tree.
+
+[![MOOD2Be](docs/video_MOOD2Be.png)](https://vimeo.com/304651183)
+
+# How to compile (plain old cmake)
+
+On Ubuntu, you are encourage to install the following dependencies:
+
+     sudo apt-get install libzmq3-dev libboost-dev
+     
+Other dependencies are already included in the __3rdparty__ folder.
+
+To compile and install the library, from the BehaviorTree.CPP folder, execute:
+
+     mkdir build; cd build
+     cmake ..
+     make
+     sudo make install
+
+If you want to use BT.CPp in your application a typical **CMakeLists.txt** file 
+will look like this:
+
+```cmake
+cmake_minimum_required(VERSION 3.5)
+
+project(hello_BT)
+
+set(CMAKE_CXX_STANDARD 14)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+find_package(BehaviorTreeV3)
+
+add_executable(${PROJECT_NAME} "hello_BT.cpp")
+target_link_libraries(${PROJECT_NAME} BT::behaviortree_cpp_v3)
+```
+
+## ROS1 or ROS2 users (Catkin/Ament)
+
+You can easily install the package with the command
+
+      sudo apt-get install ros-$ROS_DISTRO-behaviortree-cpp-v3
+      
+If you want to compile it with catkin, you __must__ include this package 
+to your catkin workspace.
+
+# Acknowledgement
+
+This library was initially developed at  **Eurecat - https://eurecat.org/en/** (main author, Davide Faconti) in a joint effort
+with the **Italian Institute of Technology** (Michele Colledanchise).
+
+This software is one of the main components of [MOOD2Be](https://eurecat.org/en/portfolio-items/mood2be/),
+which is one of the six **Integrated Technical Projects (ITPs)** selected from the
+[RobMoSys first open call](https://robmosys.eu/itp/). Therefore, MOOD2Be has been supported by the European Horizon2020 project RobMoSys. This software is RobMoSys conformant. 
+
+![RobMoSys Conformant](docs/robmosys_conformant_logo.png)
+
+# Further readings
+
+- Introductory article: [Behavior trees for AI: How they work](http://www.gamasutra.com/blogs/ChrisSimpson/20140717/221339/Behavior_trees_for_AI_How_they_work.php)
+
+- **How Behavior Trees Modularize Hybrid Control Systems and Generalize 
+Sequential Behavior Compositions, the Subsumption Architecture,
+and Decision Trees.** 
+Michele Colledanchise and Petter Ogren. IEEE Transaction on Robotics 2017.
+
+- **Behavior Trees in Robotics and AI**, 
+published by CRC Press Taylor & Francis, available for purchase
+(ebook and hardcover) on the CRC Press Store or Amazon.
+
+The Preprint version (free) is available here: https://arxiv.org/abs/1709.00084
+
+# License
+
+The MIT License (MIT)
+
+Copyright (c) 2014-2018 Michele Colledanchise
+Copyright (c) 2018-2021 Davide Faconti
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
diff --git a/cmake/FindZMQ.cmake b/cmake/FindZMQ.cmake
new file mode 100644 (file)
index 0000000..2a6baf9
--- /dev/null
@@ -0,0 +1,58 @@
+# - Try to find ZMQ
+# Once done this will define
+#
+#  ZMQ_FOUND - system has ZMQ
+#  ZMQ_INCLUDE_DIRS - the ZMQ include directory
+#  ZMQ_LIBRARIES - Link these to use ZMQ
+#  ZMQ_DEFINITIONS - Compiler switches required for using ZMQ
+#
+#  Copyright (c) 2011 Lee Hambley <lee.hambley@gmail.com>
+#
+#  Redistribution and use is allowed according to the terms of the New
+#  BSD license.
+#  For details see the accompanying COPYING-CMAKE-SCRIPTS file.
+#
+
+if (ZMQ_LIBRARIES AND ZMQ_INCLUDE_DIRS)
+  # in cache already
+  set(ZMQ_FOUND TRUE)
+else (ZMQ_LIBRARIES AND ZMQ_INCLUDE_DIRS)
+
+  find_path(ZMQ_INCLUDE_DIR
+    NAMES
+      zmq.h
+    PATHS
+      /usr/include
+      /usr/local/include
+      /opt/local/include
+  )
+
+  find_library(ZMQ_LIBRARY
+    NAMES
+      zmq
+    PATHS
+      /usr/lib
+      /usr/local/lib
+      /opt/local/lib
+      /sw/lib
+  )
+
+  set(ZMQ_INCLUDE_DIRS
+    ${ZMQ_INCLUDE_DIR}
+  )
+
+  if (ZMQ_LIBRARY)
+    set(ZMQ_LIBRARIES
+        ${ZMQ_LIBRARIES}
+        ${ZMQ_LIBRARY}
+    )
+  endif (ZMQ_LIBRARY)
+
+  include(FindPackageHandleStandardArgs)
+  find_package_handle_standard_args(ZMQ DEFAULT_MSG ZMQ_LIBRARIES ZMQ_INCLUDE_DIRS)
+
+  # show the ZMQ_INCLUDE_DIRS and ZMQ_LIBRARIES variables only in the advanced view
+  mark_as_advanced(ZMQ_INCLUDE_DIRS ZMQ_LIBRARIES)
+
+endif (ZMQ_LIBRARIES AND ZMQ_INCLUDE_DIRS)
+
diff --git a/conan/build.py b/conan/build.py
new file mode 100644 (file)
index 0000000..e86a056
--- /dev/null
@@ -0,0 +1,78 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+
+import os
+import re
+from cpt.packager import ConanMultiPackager
+from cpt.ci_manager import CIManager
+from cpt.printer import Printer
+
+
+class BuilderSettings(object):
+
+    @property
+    def branch(self):
+        """ Get branch name
+        """
+        printer = Printer(None)
+        ci_manager = CIManager(printer)
+        return ci_manager.get_branch()
+
+    @property
+    def username(self):
+        """ Set BehaviorTree as package's owner
+        """
+        return os.getenv("CONAN_USERNAME", "BehaviorTree")
+
+    @property
+    def upload(self):
+        """ Set BehaviorTree repository to be used on upload.
+            The upload server address could be customized by env var
+            CONAN_UPLOAD. If not defined, the method will check the branch name.
+            Only master or CONAN_STABLE_BRANCH_PATTERN will be accepted.
+            The master branch will be pushed to testing channel, because it does
+            not match the stable pattern. Otherwise it will upload to stable
+            channel.
+        """
+        if os.getenv("CONAN_UPLOAD", None) is not None:
+            return os.getenv("CONAN_UPLOAD")
+
+        prog = re.compile(self.stable_branch_pattern)
+        if self.branch and prog.match(self.branch):
+            return "https://api.bintray.com/conan/BehaviorTree/conan"
+
+        return None
+
+    @property
+    def upload_only_when_stable(self):
+        """ Force to upload when match stable pattern branch
+        """
+        return os.getenv("CONAN_UPLOAD_ONLY_WHEN_STABLE", True)
+
+    @property
+    def stable_branch_pattern(self):
+        """ Only upload the package the branch name is like a tag
+        """
+        return os.getenv("CONAN_STABLE_BRANCH_PATTERN", r"\d+\.\d+\.\d+")
+
+    @property
+    def version(self):
+        return self.branch if re.match(self.stable_branch_pattern, self.branch) else "latest"
+
+    @property
+    def reference(self):
+        """ Read project version from branch name to create Conan referece
+        """
+        return os.getenv("CONAN_REFERENCE", "BehaviorTree.CPP/{}".format(self.version))
+
+if __name__ == "__main__":
+    settings = BuilderSettings()
+    builder = ConanMultiPackager(
+        reference=settings.reference,
+        username=settings.username,
+        upload=settings.upload,
+        upload_only_when_stable=settings.upload_only_when_stable,
+        stable_branch_pattern=settings.stable_branch_pattern,
+        test_folder=os.path.join("conan", "test_package"))
+    builder.add_common_builds(pure_c=False)
+    builder.run()
diff --git a/conan/test_package/CMakeLists.txt b/conan/test_package/CMakeLists.txt
new file mode 100644 (file)
index 0000000..9c1c78c
--- /dev/null
@@ -0,0 +1,9 @@
+project(test_package CXX)
+cmake_minimum_required(VERSION 2.8.11)
+
+include(${CMAKE_BINARY_DIR}/conanbuildinfo.cmake)
+conan_basic_setup()
+
+add_executable(${PROJECT_NAME} test_package.cpp)
+target_link_libraries(${PROJECT_NAME} ${CONAN_LIBS})
+set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD 11)
diff --git a/conan/test_package/conanfile.py b/conan/test_package/conanfile.py
new file mode 100644 (file)
index 0000000..95695b2
--- /dev/null
@@ -0,0 +1,19 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+import os
+from conans import ConanFile, CMake
+
+
+class TestPackageConan(ConanFile):
+    settings = "os", "compiler", "build_type", "arch"
+    generators = "cmake"
+
+    def build(self):
+        cmake = CMake(self)
+        cmake.configure()
+        cmake.build()
+
+    def test(self):
+        assert os.path.isfile(os.path.join(self.deps_cpp_info["BehaviorTree.CPP"].rootpath, "licenses", "LICENSE"))
+        bin_path = os.path.join("bin", "test_package")
+        self.run(bin_path, run_environment=True)
diff --git a/conan/test_package/test_package.cpp b/conan/test_package/test_package.cpp
new file mode 100644 (file)
index 0000000..2602eac
--- /dev/null
@@ -0,0 +1,68 @@
+#include "behaviortree_cpp_v3/behavior_tree.h"
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+using namespace BT;
+
+NodeStatus SayHello()
+{
+    printf("hello\n");
+    return NodeStatus::SUCCESS;
+}
+
+class ActionTestNode : public ActionNode
+{
+  public:
+    ActionTestNode(const std::string& name) : ActionNode(name)
+    {
+    }
+
+    NodeStatus tick() override
+    {
+        time_ = 5;
+        stop_loop_ = false;
+        int i = 0;
+        while (!stop_loop_ && i++ < time_)
+        {
+            std::this_thread::sleep_for(std::chrono::milliseconds(100));
+        }
+        return NodeStatus::SUCCESS;
+    }
+
+    virtual void halt() override
+    {
+        stop_loop_ = true;
+        setStatus(NodeStatus::IDLE);
+    }
+
+  private:
+    int time_;
+    std::atomic_bool stop_loop_;
+};
+
+int main()
+{
+    BT::SequenceNode root("root");
+    BT::SimpleActionNode action1("say_hello", std::bind(SayHello));
+    ActionTestNode action2("async_action");
+
+    root.addChild(&action1);
+    root.addChild(&action2);
+
+    int count = 0;
+
+    NodeStatus status = NodeStatus::RUNNING;
+
+    while (status == NodeStatus::RUNNING)
+    {
+        status = root.executeTick();
+
+        std::cout << count++ << " : " << root.status() << " / " << action1.status() << " / "
+                  << action2.status() << std::endl;
+
+        std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    }
+
+    
+
+    return 0;
+}
diff --git a/conan/travis/build.sh b/conan/travis/build.sh
new file mode 100755 (executable)
index 0000000..069ced2
--- /dev/null
@@ -0,0 +1,14 @@
+#!/bin/bash
+
+set -e
+set -x
+
+if [[ "$(uname -s)" == 'Darwin' ]]; then
+    if which pyenv > /dev/null; then
+        eval "$(pyenv init -)"
+    fi
+    pyenv activate conan
+fi
+
+conan user
+python conan/build.py
diff --git a/conan/travis/install.sh b/conan/travis/install.sh
new file mode 100755 (executable)
index 0000000..f115909
--- /dev/null
@@ -0,0 +1,21 @@
+#!/bin/bash
+
+set -ex
+
+if [[ "$(uname -s)" == 'Darwin' ]]; then
+    brew update || brew update
+    brew outdated pyenv || brew upgrade pyenv
+    brew install pyenv-virtualenv
+    brew install cmake || true
+
+    if which pyenv > /dev/null; then
+        eval "$(pyenv init -)"
+    fi
+
+    pyenv install 3.7.1
+    pyenv virtualenv 3.7.1 conan
+    pyenv rehash
+    pyenv activate conan
+fi
+
+pip install -U conan==1.10.2 conan_package_tools
diff --git a/conanfile.py b/conanfile.py
new file mode 100644 (file)
index 0000000..311dca7
--- /dev/null
@@ -0,0 +1,70 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+
+"""Conan recipe package for BehaviorTree.CPP
+"""
+from conans import ConanFile, CMake, tools
+from conans.model.version import Version
+from conans.errors import ConanInvalidConfiguration
+
+
+class BehaviorTreeConan(ConanFile):
+    name = "BehaviorTree.CPP"
+    license = "MIT"
+    url = "https://github.com/BehaviorTree/BehaviorTree.CPP"
+    author = "Davide Faconti <davide.faconti@gmail.com>"
+    topics = ("conan", "behaviortree", "ai", "robotics", "games", "coordination")
+    description = "This C++ library provides a framework to create BehaviorTrees. It was designed to be flexible, easy to use and fast."
+    settings = "os", "compiler", "build_type", "arch"
+    options = {"shared": [True, False]}
+    default_options = {"shared": False}
+    generators = "cmake"
+    exports = "LICENSE"
+    exports_sources = ("cmake/*", "include/*", "src/*", "3rdparty/*", "CMakeLists.txt")
+    requires = "cppzmq/4.3.0@bincrafters/stable"
+
+    def configure(self):
+        if self.settings.os == "Linux" and \
+           self.settings.compiler == "gcc" and \
+           Version(self.settings.compiler.version.value) < "5":
+            raise ConanInvalidConfiguration("BehaviorTree.CPP can not be built by GCC < 5")
+        if self.settings.os == "Windows":
+            raise ConanInvalidConfiguration("BehaviorTree.CPP is not prepared to be built on Windows yet")
+
+    def _configure_cmake(self):
+        """Create CMake instance and execute configure step
+        """
+        cmake = CMake(self)
+        cmake.definitions["BUILD_EXAMPLES"] = False
+        cmake.definitions["BUILD_UNIT_TESTS"] = False
+        cmake.configure()
+        return cmake
+
+    def build(self):
+        """Configure, build and install BehaviorTree using CMake.
+        """
+        tools.replace_in_file("CMakeLists.txt",
+                              "project(behaviortree_cpp)",
+                              """project(behaviortree_cpp)
+                              include(${CMAKE_BINARY_DIR}/conanbuildinfo.cmake)
+                              conan_basic_setup()""")
+        # INFO (uilian): zmq could require libsodium
+        tools.replace_in_file("CMakeLists.txt",
+                             "BEHAVIOR_TREE_EXTERNAL_LIBRARIES zmq",
+                             "BEHAVIOR_TREE_EXTERNAL_LIBRARIES ${CONAN_LIBS}")
+        cmake = self._configure_cmake()
+        cmake.build()
+
+    def package(self):
+        """Copy BehaviorTree artifacts to package folder
+        """
+        self.copy(pattern="LICENSE", dst="licenses")
+        cmake = self._configure_cmake()
+        cmake.install()
+
+    def package_info(self):
+        """Collect built libraries names and solve pthread path.
+        """
+        self.cpp_info.libs = tools.collect_libs(self)
+        if self.settings.os == "Linux":
+            self.cpp_info.libs.append("pthread")
diff --git a/contributors.txt b/contributors.txt
new file mode 100644 (file)
index 0000000..578b30d
--- /dev/null
@@ -0,0 +1,4 @@
+Davide Faconti
+Michele Colledanchise
+Rocco Santomo
+
diff --git a/docs/BT_basics.md b/docs/BT_basics.md
new file mode 100644 (file)
index 0000000..14044e6
--- /dev/null
@@ -0,0 +1,192 @@
+# Introduction to BTs
+
+Unlike a Finite State Machine, a Behaviour Tree is a __tree of hierarchical nodes__ 
+that controls the flow of decision and the execution of "tasks" or, as we
+will call them further, "__Actions__".
+
+The __leaves__ of the tree are the actual commands, i.e. the place where
+our coordinating component interacts with the rest of the system.
+
+For instance, in a service-oriented architecture, the leaves would contain
+the "client" code that communicates with the "server" that performs the
+operation.
+
+In the following example, we can see two Actions executed in a sequence,
+`DetectObject` and `GraspObject`.
+
+![Leaf To Component Communication](images/LeafToComponentCommunication.png)
+
+The other nodes of the tree, those which are __not leaves__, control the 
+"flow of execution".
+
+To better understand how this control flow takes place, imagine a signal 
+called "__tick__"; it is executed at the __root__ of the tree and it propagates 
+through the branches until it reaches one or multiple leaves.
+
+!!! Note
+    The word __tick__ will be often used as a *verb* (to tick / to be ticked) and it means
+    
+    "To invoke the callback `tick()` of a `TreeNode`".
+
+When a `TreeNode` is ticked, it returns a `NodeStatus` that can be either:
+
+- __SUCCESS__
+- __FAILURE__
+- __RUNNING__
+
+
+The first two, as their names suggests, inform their parent that their operation
+ was a success or a failure.
+
+RUNNING is returned by __asynchronous__ nodes when their execution is not 
+completed and they needs more time to return a valid result.
+
+__Asynchronous nodes can be halted__.
+
+The result of a node is propagated back to its parent, that will decide
+which child should be ticked next or may return a result to its own parent.
+
+## Types of nodes
+
+__ControlNodes__ are nodes which can have 1 to N children. Once a tick
+is received, this tick may be propagated to one or more of the children.
+
+__DecoratorNodes__ are similar to the ControlNode, but can only have a single child. 
+
+__ActionNodes__ are leaves and do not have any children. The user should 
+implement their own ActionNodes to perform the actual tasks.
+
+__ConditionNodes__ are equivalent to ActionNodes, but
+they are always atomic and synchronous, i.e. they must not return RUNNING. 
+They should not alter the state of the system.
+
+![UML hierarchy](images/TypeHierarchy.png)
+
+
+## Examples
+
+To better understand how BehaviorTrees work, let's focus on some practical
+examples. For the sake of simplicity we will not take into account what happens
+when an action returns RUNNING.
+
+We will assume that each Action is executed atomically and synchronously.
+
+
+### First ControlNode: Sequence
+
+Let's illustrate how a BT works using the most basic and frequently used 
+ControlNode: the [SequenceNode](SequenceNode.md).
+
+The children of a ControlNode are always __ordered__; in the graphical 
+representation, the order of execution is __from left to right__.
+
+![Simple Sequence: fridge](images/SequenceBasic.png)
+
+
+In short:
+
+- If a child returns SUCCESS, tick the next one.
+- If a child returns FAILURE, then no more children are ticked and the Sequence returns FAILURE.
+- If __all__ the children return SUCCESS, then the Sequence returns SUCCESS too.
+
+!!! warning "Have you spotted the bug?"
+    If the action __GrabBeer__ fails, the door of the 
+    fridge would remain open, since the last action __CloseFridge__ is skipped.
+
+
+### Decorators
+
+Depending on the type of [DecoratorNode](DecoratorNode.md), the goal of
+this node could be either:
+
+- to transform the result it received from the child
+- to halt the execution of the child, 
+- to repeat ticking the child, depending on the type of Decorator.
+
+You can extend your grammar creating your own Decorators.
+
+![Simple Decorator: Enter Room](images/DecoratorEnterRoom.png)
+
+The node __Inverter__ is a Decorator that inverts 
+the result returned by its child; An Inverter followed by the node called
+__DoorOpen__ is therefore equivalent to 
+
+    "Is the door closed?".
+
+The node __Retry__ will repeat ticking the child up to N times (3 in this case)
+if the child returns FAILURE.
+
+__Apparently__, the branch on the right side means: 
+
+    If the door is closed, then try to open it.
+    Try up to 3 times, otherwise give up and return FAILURE.
+    
+But...
+    
+!!! warning "Have you spotted the bug?"
+    If __DoorOpen__ returns FAILURE, we have the desired behaviour.
+    But if it returns SUCCESS, the left branch fails and the entire Sequence
+    is interrupted.
+    
+    We will see later how we can improve this tree. 
+    
+
+### Second ControlNode: Fallback
+
+[FallbackNodes](FallbackNode.md), known also as __"Selectors"__,
+are nodes that can express, as the name suggests, fallback strategies, 
+i.e. what to do next if a child returns FAILURE.
+
+It ticks the children in order and:
+
+- If a child returns FAILURE, tick the next one.
+- If a child returns SUCCESS, then no more children are ticked and the 
+   Fallback returns SUCCESS.
+- If all the children return FAILURE, then the Fallback returns FAILURE too.
+
+In the next example, you can see how Sequences and Fallbacks can be combined:
+    
+![FallbackNodes](images/FallbackBasic.png)  
+
+
+> Is the door open?
+>
+> If not, try to open the door.
+>
+> Otherwise, if you have a key, unlock and open the door.
+>
+> Otherwise, smash the door. 
+>
+> If __any__ of these actions succeeded, then enter the room.
+
+### "Fetch me a beer" revisited
+
+We can now improve the "Fetch Me a Beer" example, which left the door open 
+if the beer was not inside the fridge.
+
+We use the color "green" to represent nodes which return
+SUCCESS and "red" for those which return FAILURE. Black nodes haven't
+been executed. 
+
+![FetchBeer failure](images/FetchBeerFails.png)
+
+Let's create an alternative tree that closes the door even when __GrabBeer__ 
+returns FAILURE.
+
+
+![FetchBeer failure](images/FetchBeer.png)
+
+Both these trees will close the door of the fridge, eventually, but:
+
+- the tree on the __left__ side will always return SUCCESS, no matter if
+we have actually grabbed the beer.
+- the tree on the __right__ side will return SUCCESS if the beer was there, 
+FAILURE otherwise.
+
+Everything works as expected if __GrabBeer__ returns SUCCESS.
+
+![FetchBeer success](images/FetchBeer2.png)
+
+
+
diff --git a/docs/DecoratorNode.md b/docs/DecoratorNode.md
new file mode 100644 (file)
index 0000000..24800e3
--- /dev/null
@@ -0,0 +1,45 @@
+# Decorators
+
+A decorator is a node that can have only a single child.
+
+It is up to the Decorator to decide if, when and how many times the child should be
+ticked.
+
+## InverterNode
+
+Tick the child once and return SUCCESS if the child failed or FAILURE if
+the child succeeded.
+
+If the child returns RUNNING, this node returns RUNNING too.
+
+## ForceSuccessNode
+
+If the child returns RUNNING, this node returns RUNNING too. 
+
+Otherwise, it returns always SUCCESS.
+
+## ForceFailureNode
+
+If the child returns RUNNING, this node returns RUNNING too. 
+
+Otherwise, it returns always FAILURE.
+
+## RepeatNode
+
+Tick the child up to N times, where N is passed as a [Input Port](tutorial_02_basic_ports.md),
+as long as the child returns SUCCESS.
+
+Interrupt the loop if the child returns FAILURE and, in that case, return FAILURE too.
+
+If the child returns RUNNING, this node returns RUNNING too.
+
+## RetryNode
+
+Tick the child up to N times, where N is passed as a [Input Port](tutorial_02_basic_ports.md),
+as long as the child returns FAILURE.
+
+Interrupt the loop if the child returns SUCCESS and, in that case, return SUCCESS too.
+
+If the child returns RUNNING, this node returns RUNNING too.
+
+
diff --git a/docs/FallbackNode.md b/docs/FallbackNode.md
new file mode 100644 (file)
index 0000000..3d6789a
--- /dev/null
@@ -0,0 +1,109 @@
+# Fallback
+
+This family of nodes are known as "Selector" or "Priority"
+in other frameworks.
+
+Their purpose is to try different strategies, until we find one that "works".
+
+They share the following rules:
+
+- Before ticking the first child, the node status becomes __RUNNING__.
+
+- If a child returns __FAILURE__, the fallback ticks the next child.
+
+- If the __last__ child returns __FAILURE__ too, all the children are halted and
+ the sequence returns __FAILURE__.
+- If a child returns __SUCCESS__, it stops and returns __SUCCESS__.
+  All the children are halted. 
+
+The two versions of Fallback differ in the way they react when a child returns
+RUNNING:
+
+- FallbackStar will return RUNNING and the next time it is ticked,
+ it will tick the same child where it left off before.
+- Plain old Fallback will return RUNNING and the index of the next child to
+ execute is reset after each execution.
+
+## Fallback
+
+In this example, we try different strategies to open the door. 
+Check first (and once) if the door is open.
+
+![FallbackNode](images/FallbackSimplified.png)
+
+??? example "See the pseudocode"
+       ``` c++
+               // index is initialized to 0 in the constructor
+               status = RUNNING;
+
+               while( _index < number_of_children )
+               {
+                       child_status = child[index]->tick();
+                       
+                       if( child_status == RUNNING ) {
+                               // Suspend execution and return RUNNING.
+                               // At the next tick, _index will be the same.
+                               return RUNNING;
+                       }
+                       else if( child_status == FAILURE ) {
+                               // continue the while loop
+                               _index++;
+                       }
+                       else if( child_status == SUCCESS ) {
+                               // Suspend execution and return SUCCESS.
+                           HaltAllChildren();
+                               _index = 0;
+                               return SUCCESS;
+                       }
+               }
+               // all the children returned FAILURE. Return FAILURE too.
+               index = 0;
+               HaltAllChildren();
+               return FAILURE;
+       ```     
+
+## ReactiveFallback
+
+This ControlNode is used when you want to interrupt an __asynchronous__
+child if one of the previous Conditions changes its state from 
+FAILURE to SUCCESS.
+
+In the following example, character will sleep up to 8 hours or less,
+if he/she is fully rested.
+
+![ReactiveFallback](images/ReactiveFallback.png)
+
+
+??? example "See the pseudocode"
+       ``` c++
+               // index is initialized to 0 in the constructor
+               status = RUNNING;
+
+               for (int index=0; index < number_of_children; index++)
+               {
+                       child_status = child[index]->tick();
+                       
+                       if( child_status == RUNNING ) {
+                               return RUNNING;
+                       }
+                       else if( child_status == FAILURE ) {
+                               // continue the while loop
+                               index++;
+                       }
+                       else if( child_status == SUCCESS ) {
+                               // Suspend execution and return SUCCESS.
+                               // At the next tick, index will be the same.
+                           HaltAllChildren();
+                               return SUCCESS;
+                       }
+               }
+               // all the children returned FAILURE. Return FAILURE too.
+               index = 0;
+               HaltAllChildren();
+               return FAILURE;
+       ```     
+
+
diff --git a/docs/MigrationGuide.md b/docs/MigrationGuide.md
new file mode 100644 (file)
index 0000000..0d71e04
--- /dev/null
@@ -0,0 +1,324 @@
+# Migration Guide from V2 to V3
+
+The main goal of this project is to create a Behavior Tree implementation
+that uses the principles of Model Driven Development to separate the role 
+of the __Component Developer__ from the __Behavior Designer__ and 
+__System Integrator__.
+
+In practice, this means that:
+
+- Custom Actions (or, in general, custom TreeNodes) must be __reusable__ building
+blocks. Implement them once, reuse them many times.
+
+- To build a Behavior Tree out of TreeNodes, the Behavior Designer __must 
+not need to read nor modify the source code__ of the a given TreeNode.
+
+There was a major design flaw that undermined these goals in version  `2.x`: 
+the way the BlackBoard was used to implement DataFlow between nodes.
+
+As described in [issue #18](https://github.com/BehaviorTree/BehaviorTree.CPP/issues/18)
+there are several potential problems with the Blackboard approach:
+
+- To know which entries of the BB are read/written, you should read the source code.
+
+- As a consequence, external tools such as __Groot__ can not know which BB entries are accessed.
+
+- If there is a name clashing (multiple nodes use the same key for different purposes),
+ the only way to fit it is modifying the source code. 
+
+SMACH solved this problem using [input and output ports](http://wiki.ros.org/smach/Tutorials/User%20Data)
+and remapping to connect them.
+
+In the ROS community, we potentially have the same problem with topics,
+but tools such as __rosinfo__ provides introspection at run-time and name
+clashing is avoided using remapping.
+
+This was the main reason to develop version `3.x` of __Behaviortree.CPP__, but we
+also took the opportunity to do some additional refactoring to make the code
+more understandable.
+
+In this document we will use the following terms quite often:
+
+- __Composition__: it refers to "composing" TreeNodes into Trees. In general
+ we want a TreeNode implementation to be composition-agnostic.
+- __Model/Modelling__: it is a description of a Tree or TreeNode that is 
+sufficient (and necessary) to describe it, without knowing any additional 
+detail about the actual C++ implementation.
+
+
+## Blackboard, NodeParameters an DataPorts
+
+In version `2.x` we had the intuition that passing one or more arguments
+to a `TreeNode` would make the node more generic and reusable.
+
+This is similar to the arguments of a function in any programming language.
+
+```C++
+// with arguments
+GoTo("kitchen")
+
+//Without arguments
+GoToKitchen()
+GoToLivingRoom()
+GoToBedRoom1()
+GoToBedroom2()
+// ....
+```
+
+To pass NodeParameters we used the Blackboard, that is nothing more than a
+shared __key/value__ table, i.e. a glorified bunch of global variables.
+
+The key is a `string`, whilst the value is 
+stored in a type-safe container similar to `std::any` or `std::variant`.
+
+The problem is that writing/reading in an entry of the BB was done __implicitly__
+in the source code and it was usually hard-coded. This made the TreeNode
+not reusable.
+
+To fix this, we still use the Blackboard under the hood, but it can not be 
+accessed directly anymore. 
+
+In version `3.x`Blackboard entries can be read/written using respectively 
+`InputPorts` and `OutputPorts`.
+
+These ports __must be defined explicitly__ to allow remapping at run-time.
+
+Let's take a look to an example writte using the __old__ code:
+
+```XML
+<root>
+     <BehaviorTree>
+        <SequenceStar>
+            <CalculateGoal/>
+            <MoveBase  goal="${GoalPose}" />
+        </SequenceStar>
+     </BehaviorTree>
+ </root>
+```
+
+```C++
+using namespace BT;
+//Old code (V2)
+NodeStatus CalculateGoal(TreeNode& self)
+{
+    const Pose2D mygoal = { 1, 2, 3.14};
+    // "GoalPose" is hardcoded... we don't like that
+    self.blackboard()->set("GoalPose", mygoal);
+    return NodeStatus::SUCCESS;
+}
+
+class MoveBase : public AsyncActionNode
+{
+  public:
+
+    MoveBase(const std::string& name, const NodeParameters& params)
+      : AsyncActionNode(name, params) {}
+
+    static const NodeParameters& requiredNodeParameters()
+    {
+        static NodeParameters params = {{"goal", "0;0;0"}};
+        return params;
+    }
+
+    NodeStatus tick()
+    {
+        Pose2D goal;
+        if (getParam<Pose2D>("goal", goal))
+        {
+            printf("[ MoveBase: DONE ]\n");
+            return NodeStatus::SUCCESS;
+        }
+        else{
+            printf("MoveBase: Failed for some reason\n");
+            return NodeStatus::FAILURE;
+        }
+    }
+    /// etc.
+};
+```
+
+We may notice that the `NodeParameter` can be remapped in the XML, but
+to change the key "GoalPose" in `CalculateGoalPose`we must inspect the code
+and modify it.
+
+In other words, `NodeParameter` is already a reasonably good implementation
+of an `InputPort`, but we need to introduce a consistent `OutputPort` too.
+
+This is the __new__ code:
+
+```XML
+<root>
+     <BehaviorTree>
+        <SequenceStar>
+            <CalculateGoal target="{GoalPose}" />
+            <MoveBase        goal="{GoalPose}" />
+        </SequenceStar>
+     </BehaviorTree>
+ </root>
+```
+
+```C++
+using namespace BT;
+//New code (V3)
+class CalculateGoalPose : public SyncActionNode
+{
+public:
+
+    MoveBase(const std::string& name, const NodeConfiguration& cfg)
+      : SyncActionNode(name, cfg) {}
+
+    static PortsList providedPorts()
+    {
+        return { OutputPort<Pose2D>("target") };
+    }
+
+    BT::NodeStatus tick()
+    {
+        const Pose2D myTarget = { 1, 2, 3.14 };
+        setOutput("target", myTarget);
+        return NodeStatus::SUCCESS;
+    }
+};
+
+class MoveBase : public AsyncActionNode
+{
+public:
+
+    MoveBase(const std::string& name, const NodeConfiguration& config)
+      : AsyncActionNode(name, config) {}
+
+    static PortsList providedPorts()
+    {
+        return { InputPort<Pose2D>("goal", "Port description", "0;0;0") };
+    }
+
+    NodeStatus tick()
+    {
+        Pose2D goal;
+        if (auto res = getInput<Pose2D>("goal", goal))
+        {
+            printf("[ MoveBase: DONE ]\n");
+            return NodeStatus::SUCCESS;
+        }
+        else{
+            printf("MoveBase: Failed. Error code: %s\n", res.error());
+            return NodeStatus::FAILURE;
+        }
+    }
+    /// etc.
+};
+```
+
+The main differences are:
+
+- `requiredNodeParameters()` was replaced by `providedPorts()`, that
+ is used to declare both Inputs and Output ports alike.
+   
+- `setOutput<>()` has been introduced. The method `blackboard()`can not be 
+   accessed anymore.
+
+- `getParam<>()` is now called `getInput<>()` to be more consistent with
+  `setOutput<>()`. Furthermore, if an error occurs, we can get the error 
+  message.
+  
+- Remapping to a shared entry ("GoalPose") is done at run-time in the XML.
+  You will never need to modify the C++ source code.
+
+## SubTrees, remapping and isolated Blackboards
+
+Thanks to ports we solved the problem of __reusability of single treeNodes__.
+
+But we still need to address the problem of __reusability of entire Trees/SubTrees__.
+
+According to the rule of __hierarchical composition__,
+from the point of view of a parent Node if should not matter if the 
+child is a LeafNode, a DecoratorNode a ControlNode or an entire Tree.
+
+As mentioned earlier, the Blackboard used to be a large key/value table.
+
+Unfortunately, this might be challenging when we reuse multiple SubTree, once again
+because of name clashing.
+
+The solution in version `3.x` is to have a separated and isolated Blackboard
+for each Tree/Subtree. If we want to connect the "internal" ports of a SubTree
+with the other ports of the BB of the parent, we must explicitly do a 
+remapping in the XML definition. No C++ code need to be modified.
+
+From the point of view of the XML, remapped ports of a SubTree looks exactly
+like the ports of a single node.
+
+For more details, refer to the example __t06_subtree_port_remapping.cpp__.
+
+
+## ControlNodes renamed/refactored
+
+The [principle of least astonishment](https://en.wikipedia.org/wiki/Principle_of_least_astonishment)
+applies to user interface and software design. A typical formulation of the principle, from 1984, is: 
+
+>"If a necessary feature has a high astonishment factor, it may be necessary 
+to redesign the feature.
+
+In my opinion, the two main building blocks of BehaviorTree.CPP, the `SequenceNode` 
+and the `FallbackNode` have a very high astonishment factor, because they are
+__"reactive"__.
+
+By "reactive" we mean that:
+
+- Children (usually `ConditionNodes`) that returned 
+  a valid value, such as SUCCESS or FAILURE, might be ticked again if another 
+  child returns RUNNING.
+  
+- A different result in that Condition might abort/halt the RUNNING asynchronous child.
+
+
+The main concern of the original author of this library was to build reactive
+Behavior Trees (see for reference this [publication](https://arxiv.org/abs/1709.00084)).
+
+I share this goal, but I prefer to have more explicit names, because reactive 
+ControlNodes are useful but hard to reason about sometimes.
+
+I don't think reactive ControlNodes should be used mindlessly by default. 
+
+For instance, most of the time users I talked with should have used `SequenceStar`
+instead of `Sequence` in many cases.
+
+I renamed the ControlNodes as follows to reflect this reality:
+
+
+| Old Name (v2)  |  New name (v3) | Is reactive?  |
+|:--- |:--- |:---:|
+| Sequence | ReactiveSequence  | YES  |
+| SequenceStar (reset_on_failure=true)  |  Sequence |  NO |
+| SequenceStar (reset_on_failure=false) |  SequenceStar |  NO |
+| Fallback | ReactiveFallback  | YES  |
+| FallbackStar  |  Fallback |  NO |
+| Parallel |  Parallel |  Yes(v2) / No(v3) |
+
+
+A reactive `ParallelNode` was very confusing and error prone; in most cases, 
+what you really want is you want to use a `ReactiveSequence` instead.
+
+In version `2.x` it was unclear what would happen if a "reactive" node has
+more than a single asynchronous child. 
+
+The new recommendation is: 
+
+>__Reactive nodes should NOT have more than a single asynchronous child__.
+
+This is a very opinionated decision and for this reason it is documented but 
+not enforced by the implementation.
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/docs/SequenceNode.md b/docs/SequenceNode.md
new file mode 100644 (file)
index 0000000..7b42c51
--- /dev/null
@@ -0,0 +1,149 @@
+# Sequences
+
+A __Sequence__ ticks all its children as long as 
+they return SUCCESS. If any child returns FAILURE, the sequence is aborted.
+
+Currently the framework provides three kinds of nodes:
+
+- Sequence
+- SequenceStar
+- ReactiveSequence
+
+They share the following rules:
+
+- Before ticking the first child, the node status becomes __RUNNING__.
+
+- If a child returns __SUCCESS__, it ticks the next child.
+
+- If the __last__ child returns __SUCCESS__ too, all the children are halted and
+ the sequence returns __SUCCESS__.
+
+To understand how the three ControlNodes differ, refer to the following table:
+
+| Type of ControlNode | Child returns FAILURE  |  Child returns RUNNING |
+|---|:---:|:---:|
+| Sequence | Restart  | Tick again  |
+| ReactiveSequence  | Restart  |  Restart |
+| SequenceStar | Tick again  | Tick again  |
+
+- "__Restart__" means that the entire sequence is restarted from the first 
+  child of the list.
+
+- "__Tick again__" means that the next time the sequence is ticked, the 
+  same child is ticked again. Previous sibling, which returned SUCCESS already,
+  are not ticked again.
+
+## Sequence
+
+This tree represents the behavior of a sniper in a computer game.
+
+![SequenceNode](images/SequenceNode.png)
+
+??? example "See the pseudocode"
+       ``` c++
+               status = RUNNING;
+        // _index is a private member
+
+               while(_index < number_of_children)
+               {
+                       child_status = child[_index]->tick();
+                       
+            if( child_status == SUCCESS ) {
+                _index++;
+                       }
+                       else if( child_status == RUNNING ) {
+                // keep same index
+                               return RUNNING;
+                       }
+                       else if( child_status == FAILURE ) {
+                               HaltAllChildren();
+                _index = 0;
+                               return FAILURE;
+                       }
+               }
+               // all the children returned success. Return SUCCESS too.
+               HaltAllChildren();
+        _index = 0;
+               return SUCCESS;
+       ```
+
+## ReactiveSequence
+
+This node is particularly useful to continuously check Conditions; but 
+the user should also be careful when using asynchronous children, to be
+sure that they are not ticked more often that expected.
+
+Let's take a look at another example:
+
+![ReactiveSequence](images/ReactiveSequence.png)
+
+`ApproachEnemy` is an __asynchronous__ action that returns RUNNING until
+it is, eventually, completed.
+
+The condition `isEnemyVisible` will be called many times and, 
+if it becomes false (i,e, "FAILURE"), `ApproachEnemy` is halted. 
+
+??? example "See the pseudocode"
+       ``` c++
+               status = RUNNING;
+
+               for (int index=0; index < number_of_children; index++)
+               {
+                       child_status = child[index]->tick();
+                       
+                       if( child_status == RUNNING ) {
+                               return RUNNING;
+                       }
+                       else if( child_status == FAILURE ) {
+                               HaltAllChildren();
+                               return FAILURE;
+                       }
+               }
+               // all the children returned success. Return SUCCESS too.
+               HaltAllChildren();
+               return SUCCESS;
+       ```
+
+## SequenceStar
+
+Use this ControlNode when you don't want to tick children again that 
+already returned SUCCESS.
+
+__Example__:
+
+This is a patrolling agent/robot that must visit locations A, B and C __only once__.
+If the action __GoTo(B)__ fails, __GoTo(A)__ will not be ticked again.
+
+On the other hand, __isBatteryOK__ must be checked at every tick, 
+for this reason its parent must be a `ReactiveSequence`.
+
+![SequenceStar](images/SequenceStar.png)
+
+??? example "See the pseudocode"
+       ``` c++
+               status = RUNNING;
+        // _index is a private member
+
+               while( index < number_of_children)
+               {
+                       child_status = child[index]->tick();
+                       
+            if( child_status == SUCCESS ) {
+                _index++;
+                       }
+                       else if( child_status == RUNNING || 
+                     child_status == FAILURE ) 
+            {
+                               // keep same index
+                               return child_status;
+                       }
+               }
+               // all the children returned success. Return SUCCESS too.
+               HaltAllChildren();
+        _index = 0;
+               return SUCCESS;
+       ```
+
+
diff --git a/docs/getting_started.md b/docs/getting_started.md
new file mode 100644 (file)
index 0000000..5b69558
--- /dev/null
@@ -0,0 +1,77 @@
+# Getting started
+
+__BehaviorTree.CPP__ is a C++ library that can be easily integrated into
+your favourite distributed middleware, such as __ROS__ or __SmartSoft__.
+
+You can statically link it into your application (for example a game).
+
+These are the main concepts which you need to understand first.
+
+## Nodes vs Trees
+
+The user must create his/her own ActionNodes and ConditionNodes (LeafNodes);
+this library helps you to compose them easily into trees. 
+
+Think about the LeafNodes as the building blocks which you need to compose
+a complex system.
+
+By definition, your custom Nodes are (or should be) highly __reusable__.
+But, at the beginning, some wrapping interfaces might be needed to
+adapt your legacy code.
+
+
+## The tick() callbacks
+
+Any TreeNode can be seen as a mechanism to invoke a __callback__, i.e. to 
+__run a piece of code__. What this callback does is up to you.
+
+In most of the following tutorials, our Actions will simply
+print messages on console or sleep for a certain amount of time to simulate
+a long calculation.
+
+In production code, especially in Model Driven Development and Component 
+Based Software Engineering, an Action/Condition would probably communicate
+to other _components_ or _services_ of the system.
+
+## Inheritance vs dependency injection.
+
+To create a custom TreeNode, you should inherit from the proper class.
+
+For instance, to create your own synchronous Action, you should inherit from the 
+class __SyncActionNode__.
+
+Alternatively, the library provides a mechanism to create a TreeNode passing a 
+__function pointer__ to a wrapper (dependency injection).
+
+This approach reduces the amount of boilerplate in your code; as a reference
+please look at the [first tutorial](tutorial_01_first_tree.md) and the one
+describing [non intrusive integration with legacy code](tutorial_07_legacy.md).
+
+## Dataflow, Ports and Blackboard
+
+Ports are explained in detail in the [second](tutorial_02_basic_ports.md)
+and [third](tutorial_03_generic_ports.md) tutorials.
+
+For the time being, it is important to know that:
+
+- A __Blackboard__ is a _key/value_ storage shared by all the Nodes of a Tree.
+
+- __Ports__ are a mechanism that Nodes can use to exchange information between
+  each other.
+  
+- Ports are _"connected"_ using the same _key_ of the blackboard.
+
+- The number, name and kind of ports of a Node must be known at _compilation-time_ (C++); 
+  connections between ports are done at _deployment-time_ (XML).  
+
+
+## Load trees at run-time using the XML format
+
+Despite the fact that the library is written in C++, trees themselves
+can be composed at _run-time_, more specifically, at _deployment-time_, since
+it is done only once at the beginning to instantiate the Tree.
+
+An XML format is described in details [here](xml_format.md).
+
+
+
diff --git a/docs/groot-screenshot.png b/docs/groot-screenshot.png
new file mode 100644 (file)
index 0000000..2476336
Binary files /dev/null and b/docs/groot-screenshot.png differ
diff --git a/docs/images/BT.png b/docs/images/BT.png
new file mode 100644 (file)
index 0000000..34d1c7c
Binary files /dev/null and b/docs/images/BT.png differ
diff --git a/docs/images/CrossDoorSubtree.png b/docs/images/CrossDoorSubtree.png
new file mode 100644 (file)
index 0000000..e7e32e8
Binary files /dev/null and b/docs/images/CrossDoorSubtree.png differ
diff --git a/docs/images/DecoratorEnterRoom.png b/docs/images/DecoratorEnterRoom.png
new file mode 100644 (file)
index 0000000..461504e
Binary files /dev/null and b/docs/images/DecoratorEnterRoom.png differ
diff --git a/docs/images/FallbackBasic.png b/docs/images/FallbackBasic.png
new file mode 100644 (file)
index 0000000..ea6a408
Binary files /dev/null and b/docs/images/FallbackBasic.png differ
diff --git a/docs/images/FallbackSimplified.png b/docs/images/FallbackSimplified.png
new file mode 100644 (file)
index 0000000..b7334fa
Binary files /dev/null and b/docs/images/FallbackSimplified.png differ
diff --git a/docs/images/FetchBeer.png b/docs/images/FetchBeer.png
new file mode 100644 (file)
index 0000000..fa0d15f
Binary files /dev/null and b/docs/images/FetchBeer.png differ
diff --git a/docs/images/FetchBeer2.png b/docs/images/FetchBeer2.png
new file mode 100644 (file)
index 0000000..268ca71
Binary files /dev/null and b/docs/images/FetchBeer2.png differ
diff --git a/docs/images/FetchBeerFails.png b/docs/images/FetchBeerFails.png
new file mode 100644 (file)
index 0000000..3750f09
Binary files /dev/null and b/docs/images/FetchBeerFails.png differ
diff --git a/docs/images/LeafToComponentCommunication.png b/docs/images/LeafToComponentCommunication.png
new file mode 100644 (file)
index 0000000..13a0ea2
Binary files /dev/null and b/docs/images/LeafToComponentCommunication.png differ
diff --git a/docs/images/ReactiveFallback.png b/docs/images/ReactiveFallback.png
new file mode 100644 (file)
index 0000000..6202556
Binary files /dev/null and b/docs/images/ReactiveFallback.png differ
diff --git a/docs/images/ReactiveSequence.png b/docs/images/ReactiveSequence.png
new file mode 100644 (file)
index 0000000..7626603
Binary files /dev/null and b/docs/images/ReactiveSequence.png differ
diff --git a/docs/images/ReadTheDocs.png b/docs/images/ReadTheDocs.png
new file mode 100644 (file)
index 0000000..6289129
Binary files /dev/null and b/docs/images/ReadTheDocs.png differ
diff --git a/docs/images/SequenceAll.png b/docs/images/SequenceAll.png
new file mode 100644 (file)
index 0000000..2e4ad99
Binary files /dev/null and b/docs/images/SequenceAll.png differ
diff --git a/docs/images/SequenceBasic.png b/docs/images/SequenceBasic.png
new file mode 100644 (file)
index 0000000..1468ef4
Binary files /dev/null and b/docs/images/SequenceBasic.png differ
diff --git a/docs/images/SequenceNode.png b/docs/images/SequenceNode.png
new file mode 100644 (file)
index 0000000..cc8b9ac
Binary files /dev/null and b/docs/images/SequenceNode.png differ
diff --git a/docs/images/SequenceStar.png b/docs/images/SequenceStar.png
new file mode 100644 (file)
index 0000000..889e95d
Binary files /dev/null and b/docs/images/SequenceStar.png differ
diff --git a/docs/images/TypeHierarchy.png b/docs/images/TypeHierarchy.png
new file mode 100644 (file)
index 0000000..be2c0c9
Binary files /dev/null and b/docs/images/TypeHierarchy.png differ
diff --git a/docs/images/t06_remapping.png b/docs/images/t06_remapping.png
new file mode 100644 (file)
index 0000000..3eda282
Binary files /dev/null and b/docs/images/t06_remapping.png differ
diff --git a/docs/index.md b/docs/index.md
new file mode 100644 (file)
index 0000000..c5da189
--- /dev/null
@@ -0,0 +1,90 @@
+
+# Home
+
+## About this library
+
+This  __C++__ library provides a framework to create BehaviorTrees.
+It was designed to be flexible, easy to use and fast.
+
+Even if our main use-case is __robotics__, you can use this library to build
+__AI for games__, or to replace Finite State Machines in you application.
+
+__BehaviorTree.CPP__ has many interesting features, when compared to other implementations:
+
+- It makes asynchronous Actions, i.e. non-blocking, a first-class citizen.
+- It allows the creation of trees at run-time, using a textual representation (XML).
+- You can link staticaly you custom TreeNodes or convert them into plugins 
+which are loaded at run-time.
+- It includes a __logging/profiling__ infrastructure that allows the user 
+to visualize, record, replay and analyze state transitions.
+
+![ReadTheDocs](images/ReadTheDocs.png)  
+
+## What is a Behavior Tree?
+
+A Behavior Tree (__BT__) is a way to structure the switching between different 
+tasks in an autonomous agent, such as a robot or a virtual entity in a computer game.
+
+BTs are a very efficient way of creating complex systems that are both modular and reactive. 
+These properties are crucial in many applications, which has led to the spread 
+of BT from computer game programming to many branches of AI and Robotics. 
+If you are already familiar with Finite State Machines (__FSM__), you will
+easily grasp most of the concepts but, hopefully, you will find that BTs
+are more expressive and easier to reason about.
+
+The main advantages of Behavior Trees, when compared to FSMs are:
+
+- __They are intrinsically Hierarchical__: this means that we can _compose_
+complex behaviors including entire trees as sub-branches of a bigger tree. 
+For instance, the behavior "Fetch Beer" may reuse in one of its nodes the tree
+"Grasp Object".
+
+- __Their graphical representation has a semantic meaning__: it is easier to 
+"read" a BT and understand the corresponding workflow. 
+State transitions in FSMs, by comparisons, are harder to understand
+both in their textual and graphical representation.    
+
+- __They are more expressive__: Ready to use ControlNodes and DecoratorNodes
+make possible to express more complex control flows. The user can extend the
+"vocabulary" with his/her own custom nodes.
+
+
+## "Ok, but WHY do we need BehaviorTrees (or FSM)?"
+
+Many software systems, being robotics a notable example, are inherently
+complex.
+
+The usual approach to manage complexity, heterogeneity and scalability is to 
+use the concept of 
+[Component Based Software Engineering](https://en.wikipedia.org/wiki/Component-based_software_engineering).
+
+Any existing middleware for robotics took this approach either informally or formally,
+being [ROS](http://www.ros.org), [YARP](http://www.yarp.it) and 
+[SmartSoft](http://www.servicerobotik-ulm.de) some notable examples.
+
+A "good" software architecture should have the following characteristics:
+
+- Modularity.
+- Reusability of components.
+- Composability.
+- Good separation of concerns. 
+
+If we don't keep these concepts in mind from the very beginning, we create 
+software modules/components which are highly coupled to a particular application,
+instead of being reusable.
+
+Frequently, the concern of __Coordination__ is mixed with __Computation__. 
+In other words, people address the problems of coordinating actions and take decisions
+locally.
+
+The business logic becomes "spread" in many locations and it is __hard for the developer
+to reason about it and to debug errors__ in the control flow.
+
+To achieve strong separation of concerns it is better to centralize
+the business logic in a single location. 
+
+__Finite State Machines__ were created specifically with this goal in mind, but in
+the recent years __Behavior Trees__ gained popularity, especially in the game industry.
+
+
diff --git a/docs/robmosys_conformant_logo.png b/docs/robmosys_conformant_logo.png
new file mode 100644 (file)
index 0000000..12d5ef5
Binary files /dev/null and b/docs/robmosys_conformant_logo.png differ
diff --git a/docs/tutorial_01_first_tree.md b/docs/tutorial_01_first_tree.md
new file mode 100644 (file)
index 0000000..6a5b934
--- /dev/null
@@ -0,0 +1,187 @@
+# How to create a BehaviorTree
+
+Behavior Trees, similar to State Machines, are nothing more than a mechanism
+to invoke __callbacks__ at the right time under the right conditions.
+
+Further, we will use the words __"callback"__ and __"tick"__ interchangeably.
+
+What happens inside these callbacks is up to you.
+
+In this tutorial series, most of the time Actions will just print some 
+information on console,
+but keep in mind that real "production" code would probably do something
+more complicated.
+
+## How to create your own ActionNodes
+
+The default (and recommended) way to create a TreeNode is by inheritance.
+
+``` c++
+// Example of custom SyncActionNode (synchronous action)
+// without ports.
+class ApproachObject : public BT::SyncActionNode
+{
+  public:
+    ApproachObject(const std::string& name) :
+        BT::SyncActionNode(name, {})
+    {
+    }
+
+    // You must override the virtual function tick()
+    BT::NodeStatus tick() override
+    {
+        std::cout << "ApproachObject: " << this->name() << std::endl;
+        return BT::NodeStatus::SUCCESS;
+    }
+};
+``` 
+
+As you can see:
+
+- Any instance of a TreeNode has a `name`. This identifier is meant to be 
+  human-readable and it __doesn't__ need to be unique.
+- The method __tick()__ is the place where the actual Action takes place.
+  It must always return a NodeStatus, i.e. RUNNING, SUCCESS or FAILURE. 
+
+Alternatively, we can use __dependecy injection__ to create a TreeNode given 
+a function pointer (i.e. "functor"). 
+
+The only requirement of the functor is to have either one of these signatures:
+
+``` c++
+    BT::NodeStatus myFunction()
+    BT::NodeStatus myFunction(BT::TreeNode& self) 
+```
+
+
+For example:
+
+
+``` c++
+using namespace BT;
+
+// Simple function that return a NodeStatus
+BT::NodeStatus CheckBattery()
+{
+    std::cout << "[ Battery: OK ]" << std::endl;
+    return BT::NodeStatus::SUCCESS;
+}
+
+// We want to wrap into an ActionNode the methods open() and close()
+class GripperInterface
+{
+public:
+    GripperInterface(): _open(true) {}
+    
+       NodeStatus open() {
+               _open = true;
+               std::cout << "GripperInterface::open" << std::endl;
+               return NodeStatus::SUCCESS;
+       }
+
+       NodeStatus close() {
+               std::cout << "GripperInterface::close" << std::endl;
+               _open = false;
+               return NodeStatus::SUCCESS;
+       }
+
+private:
+    bool _open; // shared information
+};
+
+``` 
+
+We can build a `SimpleActionNode` from any of these functors:
+
+- CheckBattery()
+- GripperInterface::open()
+- GripperInterface::close()
+
+## Create a tree dynamically with an XML
+
+Let's consider the following XML file named __my_tree.xml__:
+
+
+``` XML
+ <root main_tree_to_execute = "MainTree" >
+     <BehaviorTree ID="MainTree">
+        <Sequence name="root_sequence">
+            <CheckBattery   name="check_battery"/>
+            <OpenGripper    name="open_gripper"/>
+            <ApproachObject name="approach_object"/>
+            <CloseGripper   name="close_gripper"/>
+        </Sequence>
+     </BehaviorTree>
+ </root>
+```
+
+!!! Note
+    You can find more details about the XML schema [here](xml_format.md).
+
+
+We must first register our custom TreeNodes into the `BehaviorTreeFactory`
+ and then load the XML from file or text.
+
+The identifier used in the XML must coincide with those used to register
+the TreeNodes.
+
+The attribute "name" represents the name of the instance; it is optional.
+
+
+``` c++
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+// file that contains the custom nodes definitions
+#include "dummy_nodes.h"
+
+int main()
+{
+    // We use the BehaviorTreeFactory to register our custom nodes
+    BehaviorTreeFactory factory;
+
+    // Note: the name used to register should be the same used in the XML.
+    using namespace DummyNodes;
+
+    // The recommended way to create a Node is through inheritance.
+    factory.registerNodeType<ApproachObject>("ApproachObject");
+
+    // Registering a SimpleActionNode using a function pointer.
+    // you may also use C++11 lambdas instead of std::bind
+    factory.registerSimpleCondition("CheckBattery", std::bind(CheckBattery));
+
+    //You can also create SimpleActionNodes using methods of a class
+    GripperInterface gripper;
+    factory.registerSimpleAction("OpenGripper", 
+                                 std::bind(&GripperInterface::open, &gripper));
+    factory.registerSimpleAction("CloseGripper", 
+                                 std::bind(&GripperInterface::close, &gripper));
+
+    // Trees are created at deployment-time (i.e. at run-time, but only 
+    // once at the beginning). 
+    
+    // IMPORTANT: when the object "tree" goes out of scope, all the 
+    // TreeNodes are destroyed
+    auto tree = factory.createTreeFromFile("./my_tree.xml");
+
+    // To "execute" a Tree you need to "tick" it.
+    // The tick is propagated to the children based on the logic of the tree.
+    // In this case, the entire sequence is executed, because all the children
+    // of the Sequence return SUCCESS.
+    tree.tickRoot();
+
+    return 0;
+}
+
+/* Expected output:
+*
+       [ Battery: OK ]
+       GripperInterface::open
+       ApproachObject: approach_object
+       GripperInterface::close
+*/
+
+``` 
+
+
+
diff --git a/docs/tutorial_02_basic_ports.md b/docs/tutorial_02_basic_ports.md
new file mode 100644 (file)
index 0000000..d5d9aee
--- /dev/null
@@ -0,0 +1,255 @@
+# Input and Output Ports
+
+As we explained earlier, custom TreeNodes can be used to execute an arbitrarily
+simple or complex piece of software. Their goal is to provide an interface
+with a __higher level of abstraction__.
+
+For this reason, they are not conceptually different from __functions__.
+
+Similar to functions, we often want to:
+
+ - pass arguments/parameters to a Node (__inputs__)
+ - get some kind of information out from a Node (__outputs__).
+ - The outputs of a node can be the inputs of another node. 
+
+BehaviorTree.CPP provides a basic mechanism of __dataflow__
+through __ports__, that is simple to use but also flexible and type safe.
+
+## Inputs ports
+
+A valid Input can be either:
+
+- static strings which can be parsed by the Node, or
+- "pointers" to an entry of the Blackboard, identified by a __key__.
+
+A "blackboard" is a simple __key/value storage__ shared by all the nodes
+of the Tree.
+
+An "entry" of the Blackboard is a __key/value pair__.
+
+Input ports can read an entry in the Blackboard, whilst an Output port
+can write into an entry.
+
+Let's suppose that we want to create an ActionNode called `SaySomething`, 
+that should print a given string on `std::cout`.
+
+Such a string will be passed using an input port called `message`.
+
+Consider these alternative XML syntaxes:
+
+```XML
+    <SaySomething name="first"    message="hello world" />
+    <SaySomething name="second"   message="{greetings}" />
+```
+
+The attribute `message` in the __first node__ means: 
+
+    "The static string 'hello world' is passed to the port 'message' of 'SaySomething'".
+
+The message is read from the XML file, therefore it can not change at run-time.
+
+The syntax of the __second node__ instead means: 
+    
+    "Read the current value in the entry of the blackboard called 'greetings' ".
+
+This value can (and probably will) change at run-time.
+
+The ActionNode `SaySomething` can be implemented as follows:
+
+```C++
+// SyncActionNode (synchronous action) with an input port.
+class SaySomething : public SyncActionNode
+{
+  public:
+    // If your Node has ports, you must use this constructor signature 
+    SaySomething(const std::string& name, const NodeConfiguration& config)
+      : SyncActionNode(name, config)
+    { }
+
+    // It is mandatory to define this static method.
+    static PortsList providedPorts()
+    {
+        // This action has a single input port called "message"
+        // Any port must have a name. The type is optional.
+        return { InputPort<std::string>("message") };
+    }
+
+    // As usual, you must override the virtual function tick()
+    NodeStatus tick() override
+    {
+        Optional<std::string> msg = getInput<std::string>("message");
+        // Check if optional is valid. If not, throw its error
+        if (!msg)
+        {
+            throw BT::RuntimeError("missing required input [message]: ", 
+                                   msg.error() );
+        }
+
+        // use the method value() to extract the valid message.
+        std::cout << "Robot says: " << msg.value() << std::endl;
+        return NodeStatus::SUCCESS;
+    }
+};
+
+```
+
+Alternatively the same functionality can be implemented in a simple function. This function takes an instance of `BT:TreeNode` as input in order to access the "message" Input Port:
+
+```c++
+// Simple function that return a NodeStatus
+BT::NodeStatus SaySomethingSimple(BT::TreeNode& self)
+{
+  Optional<std::string> msg = self.getInput<std::string>("message");
+  // Check if optional is valid. If not, throw its error
+  if (!msg)
+  {
+    throw BT::RuntimeError("missing required input [message]: ", msg.error());
+  }
+
+  // use the method value() to extract the valid message.
+  std::cout << "Robot says: " << msg.value() << std::endl;
+  return NodeStatus::SUCCESS;
+}
+```
+
+
+
+When a custom TreeNode has input and/or output ports, these ports must be 
+declared in the __static__ method:
+
+```C++
+    static MyCustomNode::PortsList providedPorts();
+```
+
+The input from the port `message` can be read using the template method 
+`TreeNode::getInput<T>(key)`.
+
+This method may fail for multiple reasons. It is up to the user to
+check the validity of the returned value and to decide what to do:
+
+- Return `NodeStatus::FAILURE`?
+- Throw an exception?
+- Use a different default value?
+
+!!! Warning "Important"
+     It is __always__ recommended to call the method `getInput()` inside the 
+     `tick()`, and __not__ in the constructor of the class.
+     
+     The C++ code __must not make any assumption__ about 
+     the nature of the input, which could be either static or dynamic.
+     A dynamic input can change at run-time, for this reason it should be read 
+     periodically. 
+
+## Output ports
+
+An input port pointing to the entry of the blackboard will be valid only
+if another node have already written "something" inside that same entry.
+
+`ThinkWhatToSay` is an example of Node that uses an __output port__ to write a 
+string into an entry.
+
+```C++
+class ThinkWhatToSay : public SyncActionNode
+{
+  public:
+    ThinkWhatToSay(const std::string& name, const NodeConfiguration& config)
+      : SyncActionNode(name, config)
+    {
+    }
+
+    static PortsList providedPorts()
+    {
+        return { OutputPort<std::string>("text") };
+    }
+
+    // This Action writes a value into the port "text"
+    NodeStatus tick() override
+    {
+        // the output may change at each tick(). Here we keep it simple.
+        setOutput("text", "The answer is 42" );
+        return NodeStatus::SUCCESS;
+    }
+};
+```
+
+Alternatively, most of the time for debugging purposes, it is possible to write a
+static value into an entry using the built-in Actions called `SetBlackboard`.
+
+```XML
+ <SetBlackboard   output_key="the_answer" value="The answer is 42" />
+```
+
+## A complete example
+
+In this example, a Sequence of 5 Actions is executed:
+
+- Actions 1 and 4 read the input `message` from a static string.
+
+- Actions 3 and 5 read the input `message` from an entry in the
+  blackboard called `the_answer`.
+
+- Action 2 writes something into the entry of the blackboard called `the_answer`.
+
+`SaySomething2` is a SimpleActionNode. 
+
+```XML
+<root main_tree_to_execute = "MainTree" >
+    <BehaviorTree ID="MainTree">
+       <Sequence name="root_sequence">
+           <SaySomething     message="start thinking..." />
+           <ThinkWhatToSay   text="{the_answer}"/>
+           <SaySomething     message="{the_answer}" />
+           <SaySomething2    message="SaySomething2 works too..." />
+           <SaySomething2    message="{the_answer}" />
+       </Sequence>
+    </BehaviorTree>
+</root>
+```
+
+The C++ code:
+
+```C++
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+// file that contains the custom nodes definitions
+#include "dummy_nodes.h"
+
+int main()
+{
+    using namespace DummyNodes;
+    
+    BehaviorTreeFactory factory;
+
+    factory.registerNodeType<SaySomething>("SaySomething");
+    factory.registerNodeType<ThinkWhatToSay>("ThinkWhatToSay");
+
+    // SimpleActionNodes can not define their own method providedPorts().
+    // We should pass a PortsList explicitly if we want the Action to 
+    // be able to use getInput() or setOutput();
+    PortsList say_something_ports = { InputPort<std::string>("message") };
+    factory.registerSimpleAction("SaySomething2", SaySomethingSimple, 
+                                 say_something_ports );
+
+    auto tree = factory.createTreeFromFile("./my_tree.xml");
+
+    tree.tickRoot();
+
+    /*  Expected output:
+
+        Robot says: start thinking...
+        Robot says: The answer is 42
+        Robot says: SaySomething2 works too...
+        Robot says: The answer is 42
+    */
+    return 0;
+}
+```
+
+We "connect" output ports to input ports using the same key (`the_aswer`);
+this means that they "point" to the same entry of the blackboard.
+
+These ports can be connected to each other because their type is the same,
+i.e. `std::string`.
+
+
+
diff --git a/docs/tutorial_03_generic_ports.md b/docs/tutorial_03_generic_ports.md
new file mode 100644 (file)
index 0000000..d5d20df
--- /dev/null
@@ -0,0 +1,185 @@
+# Ports with generic types
+
+In the previous tutorials we introduced input and output ports, where the
+type of the port itself was a `std::string`.
+
+This is the easiest port type to deal with, because any parameter passed
+from the XML definition will be (obviously) a string.
+
+Next, we will describe how to use any generic C++ type in your code.
+
+## Parsing a string
+
+__BehaviorTree.CPP__ supports automatic conversion of strings into common
+types, such as `int`, `long`, `double`, `bool`, `NodeStatus`, etc.
+
+But user defined types can be supported easily as well. For instance:
+
+```C++
+// We want to be able to use this custom type
+struct Position2D 
+{ 
+  double x;
+  double y; 
+};
+```
+
+To parse a string into a `Position2D` we should link to a template 
+specialization of `BT::convertFromString<Position2D>(StringView)`.
+
+We can use any syntax we want; in this case, we simply separate two numbers
+with a _semicolon_.
+
+
+```C++
+// Template specialization to converts a string to Position2D.
+namespace BT
+{
+    template <> inline Position2D convertFromString(StringView str)
+    {
+        // The next line should be removed...
+        printf("Converting string: \"%s\"\n", str.data() );
+
+        // We expect real numbers separated by semicolons
+        auto parts = splitString(str, ';');
+        if (parts.size() != 2)
+        {
+            throw RuntimeError("invalid input)");
+        }
+        else{
+            Position2D output;
+            output.x     = convertFromString<double>(parts[0]);
+            output.y     = convertFromString<double>(parts[1]);
+            return output;
+        }
+    }
+} // end namespace BT
+```
+
+About the previous code:
+
+- `StringView` is just a C++11 version of [std::string_view](https://en.cppreference.com/w/cpp/header/string_view). 
+   You can pass either a `std::string` or a `const char*`.
+-  In production code, you would (obviously) remove the `printf` statement.
+-  The library provides a simple `splitString` function. Feel free to use another
+   one, like [boost::algorithm::split](onvertFromString<double>).
+-  Once we split the input into single numbers, we can reuse the specialization 
+   `convertFromString<double>()`.  
+   
+## Example
+
+Similarly to the previous tutorial, we can create two custom Actions,
+one will write into a port and the other will read from a port.
+
+
+```C++
+
+class CalculateGoal: public SyncActionNode
+{
+public:
+    CalculateGoal(const std::string& name, const NodeConfiguration& config):
+        SyncActionNode(name,config)
+    {}
+
+    static PortsList providedPorts()
+    {
+        return { OutputPort<Position2D>("goal") };
+    }
+
+    NodeStatus tick() override
+    {
+        Position2D mygoal = {1.1, 2.3};
+        setOutput<Position2D>("goal", mygoal);
+        return NodeStatus::SUCCESS;
+    }
+};
+
+
+class PrintTarget: public SyncActionNode
+{
+public:
+    PrintTarget(const std::string& name, const NodeConfiguration& config):
+        SyncActionNode(name,config)
+    {}
+
+    static PortsList providedPorts()
+    {
+        // Optionally, a port can have a human readable description
+        const char*  description = "Simply print the goal on console...";
+        return { InputPort<Position2D>("target", description) };
+    }
+    
+    NodeStatus tick() override
+    {
+        auto res = getInput<Position2D>("target");
+        if( !res )
+        {
+            throw RuntimeError("error reading port [target]:", res.error());
+        }
+        Position2D target = res.value();
+        printf("Target positions: [ %.1f, %.1f ]\n", target.x, target.y );
+        return NodeStatus::SUCCESS;
+    }
+};
+```   
+
+We can now connect input/output ports as usual, pointing to the same 
+entry of the Blackboard.
+
+The tree in the next example is a Sequence of 4 actions
+
+- Store a value of `Position2D` in the entry "GoalPosition",
+  using the action `CalculateGoal`.
+
+- Call `PrintTarget`. The input "target" will be read from the Blackboard
+  entry "GoalPosition".
+
+- Use the built-in action `SetBlackboard` to write the key "OtherGoal".
+  A conversion from string to `Position2D` will be done under the hood.
+
+- Call `PrintTarget` again. The input "target" will be read from the Blackboard
+  entry "OtherGoal".
+
+
+```C++  
+static const char* xml_text = R"(
+
+ <root main_tree_to_execute = "MainTree" >
+     <BehaviorTree ID="MainTree">
+        <SequenceStar name="root">
+            <CalculateGoal   goal="{GoalPosition}" />
+            <PrintTarget     target="{GoalPosition}" />
+            <SetBlackboard   output_key="OtherGoal" value="-1;3" />
+            <PrintTarget     target="{OtherGoal}" />
+        </SequenceStar>
+     </BehaviorTree>
+ </root>
+ )";
+
+int main()
+{
+    using namespace BT;
+
+    BehaviorTreeFactory factory;
+    factory.registerNodeType<CalculateGoal>("CalculateGoal");
+    factory.registerNodeType<PrintTarget>("PrintTarget");
+
+    auto tree = factory.createTreeFromText(xml_text);
+    tree.tickRoot();
+
+/* Expected output:
+
+    Target positions: [ 1.1, 2.3 ]
+    Converting string: "-1;3"
+    Target positions: [ -1.0, 3.0 ]
+*/
+    return 0;
+}
+```  
+
+
+
+
+
+
+   
diff --git a/docs/tutorial_04_sequence_star.md b/docs/tutorial_04_sequence_star.md
new file mode 100644 (file)
index 0000000..4c59c0c
--- /dev/null
@@ -0,0 +1,188 @@
+# Sequences and AsyncActionNode
+
+The next example shows the difference between a `SequenceNode` and a 
+`ReactiveSequence`.
+
+An Asynchronous Action has it's own thread. This allows the user to 
+use blocking functions but to return the flow of execution 
+to the tree.
+
+```C++
+
+// Custom type
+struct Pose2D
+{
+    double x, y, theta;
+};
+
+class MoveBaseAction : public AsyncActionNode
+{
+  public:
+    MoveBaseAction(const std::string& name, const NodeConfiguration& config)
+      : AsyncActionNode(name, config)
+    { }
+
+    static PortsList providedPorts()
+    {
+        return{ InputPort<Pose2D>("goal") };
+    }
+
+    NodeStatus tick() override;
+
+    // This overloaded method is used to stop the execution of this node.
+    void halt() override
+    {
+        _halt_requested.store(true);
+    }
+
+  private:
+    std::atomic_bool _halt_requested;
+};
+
+//-------------------------
+
+NodeStatus MoveBaseAction::tick()
+{
+    Pose2D goal;
+    if ( !getInput<Pose2D>("goal", goal))
+    {
+        throw RuntimeError("missing required input [goal]");
+    }
+
+    printf("[ MoveBase: STARTED ]. goal: x=%.f y=%.1f theta=%.2f\n", 
+           goal.x, goal.y, goal.theta);
+
+    _halt_requested.store(false);
+    int count = 0;
+
+    // Pretend that "computing" takes 250 milliseconds.
+    // It is up to you to check periodicall _halt_requested and interrupt
+    // this tick() if it is true.
+    while (!_halt_requested && count++ < 25)
+    {
+        SleepMS(10);
+    }
+
+    std::cout << "[ MoveBase: FINISHED ]" << std::endl;
+    return _halt_requested ? NodeStatus::FAILURE : NodeStatus::SUCCESS;
+}
+```
+
+The method `MoveBaseAction::tick()` is executed in a thread different from the 
+main thread that invoked `MoveBaseAction::executeTick()`.
+
+__You are responsible__ for the implementation of a valid __halt()__ functionality.
+
+The user must also implement `convertFromString<Pose2D>(StringView)`,
+as shown in the previous tutorial.
+
+
+## Sequence VS ReactiveSequence
+
+The following example should use a simple `SequenceNode`.
+
+```XML hl_lines="3"
+ <root>
+     <BehaviorTree>
+        <Sequence>
+            <BatteryOK/>
+            <SaySomething   message="mission started..." />
+            <MoveBase       goal="1;2;3"/>
+            <SaySomething   message="mission completed!" />
+        </Sequence>
+     </BehaviorTree>
+ </root>
+```
+
+```C++
+int main()
+{
+    using namespace DummyNodes;
+
+    BehaviorTreeFactory factory;
+    factory.registerSimpleCondition("BatteryOK", std::bind(CheckBattery));
+    factory.registerNodeType<MoveBaseAction>("MoveBase");
+    factory.registerNodeType<SaySomething>("SaySomething");
+
+    auto tree = factory.createTreeFromText(xml_text);
+
+    NodeStatus status;
+
+    std::cout << "\n--- 1st executeTick() ---" << std::endl;
+    status = tree.tickRoot();
+
+    SleepMS(150);
+    std::cout << "\n--- 2nd executeTick() ---" << std::endl;
+    status = tree.tickRoot();
+
+    SleepMS(150);
+    std::cout << "\n--- 3rd executeTick() ---" << std::endl;
+    status = tree.tickRoot();
+
+    std::cout << std::endl;
+
+    return 0;
+}
+```
+
+Expected output:
+
+``` 
+    --- 1st executeTick() ---
+    [ Battery: OK ]
+    Robot says: "mission started..."
+    [ MoveBase: STARTED ]. goal: x=1 y=2.0 theta=3.00
+
+    --- 2nd executeTick() ---
+    [ MoveBase: FINISHED ]
+
+    --- 3rd executeTick() ---
+    Robot says: "mission completed!"
+```
+
+You may have noticed that when `executeTick()` was called, `MoveBase` returned
+__RUNNING__ the 1st and 2nd time, and eventually __SUCCESS__ the 3rd time.
+
+`BatteryOK` is executed only once. 
+
+If we use a `ReactiveSequence` instead, when the child `MoveBase` returns RUNNING,
+the sequence is restarted and the condition `BatteryOK` is executed __again__.
+
+If, at any point, `BatteryOK` returned __FAILURE__, the `MoveBase` action
+would be _interrupted_ (_halted_, to be specific).
+
+```XML hl_lines="3"
+ <root>
+     <BehaviorTree>
+        <ReactiveSequence>
+            <BatteryOK/>
+            <Sequence>
+                <SaySomething   message="mission started..." />
+                <MoveBase       goal="1;2;3"/>
+                <SaySomething   message="mission completed!" />
+            </Sequence>
+        </ReactiveSequence>
+     </BehaviorTree>
+ </root>
+```
+
+
+Expected output:
+
+``` 
+    --- 1st executeTick() ---
+    [ Battery: OK ]
+    Robot says: "mission started..."
+    [ MoveBase: STARTED ]. goal: x=1 y=2.0 theta=3.00
+
+    --- 2nd executeTick() ---
+    [ Battery: OK ]
+    [ MoveBase: FINISHED ]
+
+    --- 3rd executeTick() ---
+    [ Battery: OK ]
+    Robot says: "mission completed!"
+```
+
+
+
diff --git a/docs/tutorial_05_subtrees.md b/docs/tutorial_05_subtrees.md
new file mode 100644 (file)
index 0000000..fd7d532
--- /dev/null
@@ -0,0 +1,124 @@
+# Composition of Behaviors with Subtree
+
+We can build large scale behavior composing together smaller and reusable
+behaviors into larger ones.
+
+In other words, we want to create __hierarchical__ behavior trees. 
+
+This can be achieved easily defining multiple trees in the XML including one
+into the other.
+
+# CrossDoor behavior
+
+This example is inspired by a popular 
+[article about behavior trees](http://www.gamasutra.com/blogs/ChrisSimpson/20140717/221339/Behavior_trees_for_AI_How_they_work.php).
+
+It is also the first practical example that uses `Decorators` and `Fallback`.
+
+```XML hl_lines="1 3 15"
+<root main_tree_to_execute = "MainTree">
+       
+    <BehaviorTree ID="DoorClosed">
+        <Sequence name="door_closed_sequence">
+            <Inverter>
+                <IsDoorOpen/>
+            </Inverter>
+            <RetryUntilSuccesful num_attempts="4">
+                <OpenDoor/>
+            </RetryUntilSuccesful>
+            <PassThroughDoor/>
+        </Sequence>
+    </BehaviorTree>
+    
+    <BehaviorTree ID="MainTree">
+        <Fallback name="root_Fallback">
+            <Sequence name="door_open_sequence">
+                <IsDoorOpen/>
+                <PassThroughDoor/>
+            </Sequence>
+            <SubTree ID="DoorClosed"/>
+            <PassThroughWindow/>
+        </Fallback>
+    </BehaviorTree>
+    
+</root>
+```
+
+It may be noticed that we incapsulated a quite complex branch of the tree,
+the one to execute when the door is closed, into a separate tree called
+`DoorClosed`.
+
+The desired behavior is:
+
+- If the door is open, `PassThroughDoor`.
+- If the door is closed, try up to 4 times to `OpenDoor` and, then, `PassThroughDoor`.
+- If it was not possible to open the closed door, `PassThroughWindow`.
+
+
+## Loggers
+
+On the C++ side we don't need to do anything to build reusable subtrees.
+
+Therefore we take this opportunity to introduce another neat feature of
+_BehaviorTree.CPP_ : __Loggers__.
+
+A Logger is a mechanism to display, record and/or publish any state change in the tree.
+
+
+```C++
+
+int main()
+{
+    using namespace BT;
+    BehaviorTreeFactory factory;
+
+    // register all the actions into the factory
+    // We don't show how these actions are implemented, since most of the 
+    // times they just print a message on screen and return SUCCESS.
+    // See the code on Github for more details.
+    factory.registerSimpleCondition("IsDoorOpen", std::bind(IsDoorOpen));
+    factory.registerSimpleAction("PassThroughDoor", std::bind(PassThroughDoor));
+    factory.registerSimpleAction("PassThroughWindow", std::bind(PassThroughWindow));
+    factory.registerSimpleAction("OpenDoor", std::bind(OpenDoor));
+    factory.registerSimpleAction("CloseDoor", std::bind(CloseDoor));
+    factory.registerSimpleCondition("IsDoorLocked", std::bind(IsDoorLocked));
+    factory.registerSimpleAction("UnlockDoor", std::bind(UnlockDoor));
+
+    // Load from text or file...
+    auto tree = factory.createTreeFromText(xml_text);
+
+    // This logger prints state changes on console
+    StdCoutLogger logger_cout(tree);
+
+    // This logger saves state changes on file
+    FileLogger logger_file(tree, "bt_trace.fbl");
+    
+    // This logger stores the execution time of each node
+    MinitraceLogger logger_minitrace(tree, "bt_trace.json");
+
+#ifdef ZMQ_FOUND
+    // This logger publish status changes using ZeroMQ. Used by Groot
+    PublisherZMQ publisher_zmq(tree);
+#endif
+
+    printTreeRecursively(tree.rootNode());
+
+    //while (1)
+    {
+        NodeStatus status = NodeStatus::RUNNING;
+        // Keep on ticking until you get either a SUCCESS or FAILURE state
+        while( status == NodeStatus::RUNNING)
+        {
+            status = tree.tickRoot();
+            CrossDoor::SleepMS(1);   // optional sleep to avoid "busy loops"
+        }
+        CrossDoor::SleepMS(2000);
+    }
+    return 0;
+}
+
+```
+
+
+
+
diff --git a/docs/tutorial_06_subtree_ports.md b/docs/tutorial_06_subtree_ports.md
new file mode 100644 (file)
index 0000000..b9c5c0a
--- /dev/null
@@ -0,0 +1,111 @@
+# Remapping ports between Trees and SubTrees 
+
+In the CrossDoor example we saw that a `SubTree` looks like a single
+leaf Node from the point of view of its parent (`MainTree` in the example).
+
+Furthermore, to avoid name clashing in very large trees, any tree and subtree
+use a different instance of the Blackboard.
+
+For this reason, we need to explicitly connect the ports of a tree to those
+of its subtrees.
+
+Once again, you __won't__ need to modify your C++ implementation since this 
+remapping is done entirely in the XML definition.
+
+## Example
+
+Let's consider this Beahavior Tree.
+
+```XML hl_lines="7"
+<root main_tree_to_execute = "MainTree">
+
+    <BehaviorTree ID="MainTree">
+
+        <Sequence name="main_sequence">
+            <SetBlackboard output_key="move_goal" value="1;2;3" />
+            <SubTree ID="MoveRobot" target="move_goal" output="move_result" />
+            <SaySomething message="{move_result}"/>
+        </Sequence>
+
+    </BehaviorTree>
+
+    <BehaviorTree ID="MoveRobot">
+        <Fallback name="move_robot_main">
+            <SequenceStar>
+                <MoveBase       goal="{target}"/>
+                <SetBlackboard output_key="output" value="mission accomplished" />
+            </SequenceStar>
+            <ForceFailure>
+                <SetBlackboard output_key="output" value="mission failed" />
+            </ForceFailure>
+        </Fallback>
+    </BehaviorTree>
+
+</root>
+```
+
+You may notice that:
+
+- We have a `MainTree` that includes a subtree called `MoveRobot`.
+- We want to "connect" (i.e. "remap") ports inside the `MoveRobot` subtree
+with other ports in the `MainTree`.
+- This is done using the XMl tag __<remap>__, where the words __internal/external__
+  refer respectively to a subtree and its parent.
+
+
+The following image shows remapping between these two different trees.
+
+Note that this diagram represents the __dataflow__ and the entries in the
+respective blackboard, not the relationship in terms of Behavior Trees.
+
+![ports remapping](images/t06_remapping.png)
+
+In terms of C++, we don't need to do much. For debugging purpose, we may show some
+information about the current state of a blackboard with the method `debugMessage()`.
+
+```C++
+int main()
+{
+    BT::BehaviorTreeFactory factory;
+
+    factory.registerNodeType<SaySomething>("SaySomething");
+    factory.registerNodeType<MoveBaseAction>("MoveBase");
+
+    auto tree = factory.createTreeFromText(xml_text);
+
+    NodeStatus status = NodeStatus::RUNNING;
+    // Keep on ticking until you get either a SUCCESS or FAILURE state
+    while( status == NodeStatus::RUNNING)
+    {
+        status = tree.tickRoot();
+        SleepMS(1);   // optional sleep to avoid "busy loops"
+    }
+
+    // let's visualize some information about the current state of the blackboards.
+    std::cout << "--------------" << std::endl;
+    tree.blackboard_stack[0]->debugMessage();
+    std::cout << "--------------" << std::endl;
+    tree.blackboard_stack[1]->debugMessage();
+    std::cout << "--------------" << std::endl;
+
+    return 0;
+}
+
+/* Expected output:
+
+    [ MoveBase: STARTED ]. goal: x=1 y=2.0 theta=3.00
+    [ MoveBase: FINISHED ]
+    Robot says: mission accomplished
+    --------------
+    move_result (std::string) -> full
+    move_goal (Pose2D) -> full
+    --------------
+    output (std::string) -> remapped to parent [move_result]
+    target (Pose2D) -> remapped to parent [move_goal]
+    --------------
+*/
+```
+
+
+
+
diff --git a/docs/tutorial_07_legacy.md b/docs/tutorial_07_legacy.md
new file mode 100644 (file)
index 0000000..1cee725
--- /dev/null
@@ -0,0 +1,104 @@
+# Wraping legacy code
+
+In this tutorial we will see how to deal with legacy code that was not meant to be used
+with BehaviorTree.CPP.
+
+Your class might look like this one:
+
+``` C++
+// This is my custom type.
+struct Point3D { double x,y,z; };
+
+// We want to create an ActionNode to calls method MyLegacyMoveTo::go
+class MyLegacyMoveTo
+{
+public:
+    bool go(Point3D goal)
+    {
+        printf("Going to: %f %f %f\n", goal.x, goal.y, goal.z);
+        return true; // true means success in my legacy code
+    }
+};
+```
+
+## C++ code
+
+As usuall, we need to implement the template specialization of `convertFromString`.
+
+``` C++
+namespace BT
+{
+    template <> Point3D convertFromString(StringView key)
+    {
+        // three real numbers separated by semicolons
+        auto parts = BT::splitString(key, ';');
+        if (parts.size() != 3)
+        {
+            throw RuntimeError("invalid input)");
+        }
+        else{
+            Point3D output;
+            output.x  = convertFromString<double>(parts[0]);
+            output.y  = convertFromString<double>(parts[1]);
+            output.z  = convertFromString<double>(parts[2]);
+            return output;
+        }
+    }
+} // end anmespace BT
+```
+
+To wrap the method `MyLegacyMoveTo::go`, we may use a __lambda or std::bind__ 
+to create a funtion pointer and `SimpleActionNode`.
+
+```C++
+static const char* xml_text = R"(
+
+ <root>
+     <BehaviorTree>
+        <MoveTo  goal="-1;3;0.5" />
+     </BehaviorTree>
+ </root>
+ )";
+
+int main()
+{
+    using namespace BT;
+
+    MyLegacyMoveTo move_to;
+
+    // Here we use a lambda that captures the reference of move_to
+    auto MoveToWrapperWithLambda = [&move_to](TreeNode& parent_node) -> NodeStatus
+    {
+        Point3D goal;
+        // thanks to paren_node, you can access easily the input and output ports.
+        parent_node.getInput("goal", goal);
+
+        bool res = move_to.go( goal );
+        // convert bool to NodeStatus
+        return res ? NodeStatus::SUCCESS : NodeStatus::FAILURE;
+    };
+
+    BehaviorTreeFactory factory;
+
+    // Register the lambda with BehaviorTreeFactory::registerSimpleAction
+
+    PortsList ports = { BT::InputPort<Point3D>("goal") };
+    factory.registerSimpleAction("MoveTo", MoveToWrapperWithLambda, ports );
+
+    auto tree = factory.createTreeFromText(xml_text);
+
+    tree.tickRoot();
+
+    return 0;
+}
+
+/* Expected output:
+
+Going to: -1.000000 3.000000 0.500000
+
+*/
+```
+
+
+
diff --git a/docs/tutorial_08_additional_args.md b/docs/tutorial_08_additional_args.md
new file mode 100644 (file)
index 0000000..ce2aef3
--- /dev/null
@@ -0,0 +1,153 @@
+# Pass additional arguments during initialization and/or construction
+
+In every single example we explored so far we were "forced" to provide a
+constructor with the following signature
+
+```C++
+    MyCustomNode(const std::string& name, const NodeConfiguration& config);
+
+```
+
+In same cases, it is desirable to pass to the constructor of our class 
+additional arguments, parameters, pointers, references, etc.
+
+**Many people use blackboards to do that: this is not recomendable.**
+
+We will just use the word _"arguments"_ for the rest of the tutorial.
+
+Even if, theoretically, these arguments **could** be passed using Input Ports,
+that would be the wrong way to do it if:
+
+- The arguments are known at _deployment-time_.
+- The arguments don't change at _run-time_.
+- The arguments don't need to be set from the _XML_.
+
+If all these conditions are met, using ports or the blackboard is cumbersome and highly discouraged.
+
+## Method 1: register a custom builder
+
+Consider the following custom node called **Action_A**.
+
+We want to pass three additional arguments; they can be arbitrarily complex objects,
+you are not limited to built-in types.
+
+```C++
+// Action_A has a different constructor than the default one.
+class Action_A: public SyncActionNode
+{
+
+public:
+    // additional arguments passed to the constructor
+    Action_A(const std::string& name, const NodeConfiguration& config,
+             int arg1, double arg2, std::string arg3 ):
+        SyncActionNode(name, config),
+        _arg1(arg1),
+        _arg2(arg2),
+        _arg3(arg3) {}
+
+    // this example doesn't require any port
+    static PortsList providedPorts() { return {}; }
+
+    // tick() can access the private members
+    NodeStatus tick() override;
+
+private:
+    int _arg1;
+    double _arg2;
+    std::string _arg3;
+};
+```
+
+This node should be registered as shown further:
+
+```C++
+BehaviorTreeFactory factory;
+
+// A node builder is a functor that creates a std::unique_ptr<TreeNode>.
+// Using lambdas or std::bind, we can easily "inject" additional arguments.
+NodeBuilder builder_A =
+   [](const std::string& name, const NodeConfiguration& config)
+{
+    return std::make_unique<Action_A>( name, config, 42, 3.14, "hello world" );
+};
+
+// BehaviorTreeFactory::registerBuilder is a more general way to
+// register a custom node.
+factory.registerBuilder<Action_A>( "Action_A", builder_A);
+
+// Register more custom nodes, if needed.
+// ....
+
+// The rest of your code, where you create and tick the tree, goes here.
+// ....
+```
+
+## Method 2: use an init method
+
+Alternatively, you may call an init method before ticking the tree.
+
+```C++
+
+class Action_B: public SyncActionNode
+{
+
+public:
+    // The constructor looks as usual.
+    Action_B(const std::string& name, const NodeConfiguration& config):
+        SyncActionNode(name, config) {}
+
+    // We want this method to be called ONCE and BEFORE the first tick()
+    void init( int arg1, double arg2, const std::string& arg3 )
+    {
+        _arg1 = (arg1);
+        _arg2 = (arg2);
+        _arg3 = (arg3);
+    }
+
+    // this example doesn't require any port
+    static PortsList providedPorts() { return {}; }
+
+    // tick() can access the private members
+    NodeStatus tick() override;
+
+private:
+    int _arg1;
+    double _arg2;
+    std::string _arg3;
+};
+```
+
+The way we register and initialize Action_B is slightly different:
+
+```C++
+
+BehaviorTreeFactory factory;
+
+// The regitration of  Action_B is done as usual, but remember
+// that we still need to call Action_B::init()
+factory.registerNodeType<Action_B>( "Action_B" );
+
+// Register more custom nodes, if needed.
+// ....
+
+// Create the whole tree
+auto tree = factory.createTreeFromText(xml_text);
+
+// Iterate through all the nodes and call init() if it is an Action_B
+for( auto& node: tree.nodes )
+{
+    // Not a typo: it is "=", not "=="
+    if( auto action_B = dynamic_cast<Action_B*>( node.get() ))
+    {
+        action_B->init( 42, 3.14, "hello world");
+    }
+}
+
+// The rest of your code, where you tick the tree, goes here.
+// ....
+```
+
+
+
+
+
diff --git a/docs/tutorial_09_coroutines.md b/docs/tutorial_09_coroutines.md
new file mode 100644 (file)
index 0000000..6458e4f
--- /dev/null
@@ -0,0 +1,171 @@
+# Async Actions using Coroutines
+
+BehaviorTree.CPP provides two easy-to-use abstractions to create an 
+asynchronous Action, i.e. those actions which:
+
+- Take a long time to be concluded.
+- May return "RUNNING".
+- Can be __halted__.
+
+The first class is a __AsyncActionNode__ that executes the tick() method in a
+_separate thread_.
+
+In this tutorial, we introduce the __CoroActionNode__, a different action that uses
+[coroutines](https://www.geeksforgeeks.org/coroutines-in-c-cpp/) 
+to achieve similar results.
+
+The main reason is that Coroutines do not spawn a new thread and are much more efficient.
+Furthermore, you don't need to worry about thread-safety in your code...
+
+In Coroutines, the user should explicitly call a __yield__ method when 
+he/she wants the execution of the Action to be suspended.
+
+`CoroActionNode` wraps this `yield` function into a convenient method 
+`setStatusRunningAndYield()`. 
+
+## The C++ source example
+
+The next example can be used as a "template" for your own implementation.
+
+
+``` c++
+
+typedef std::chrono::milliseconds Milliseconds;
+
+class MyAsyncAction: public CoroActionNode
+{
+  public:
+    MyAsyncAction(const std::string& name):
+        CoroActionNode(name, {})
+    {}
+
+  private:
+    // This is the ideal skeleton/template of an async action:
+    //  - A request to a remote service provider.
+    //  - A loop where we check if the reply has been received.
+    //  - You may call setStatusRunningAndYield() to "pause".
+    //  - Code to execute after the reply.
+    //  - A simple way to handle halt().
+    NodeStatus tick() override
+    {
+        std::cout << name() <<": Started. Send Request to server." << std::endl;
+
+        TimePoint initial_time = Now();
+        TimePoint time_before_reply = initial_time + Milliseconds(100);
+
+        int count = 0;
+        bool reply_received = false;
+
+        while( !reply_received )
+        {
+            if( count++ == 0)
+            {
+                // call this only once
+                std::cout << name() <<": Waiting Reply..." << std::endl;
+            }
+            // pretend that we received a reply
+            if( Now() >= time_before_reply )
+            {
+                reply_received = true;
+            }
+
+            if( !reply_received )
+            {
+                // set status to RUNNING and "pause/sleep"
+                // If halt() is called, we will NOT resume execution
+                setStatusRunningAndYield();
+            }
+        }
+
+        // This part of the code is never reached if halt() is invoked,
+        // only if reply_received == true;
+        std::cout << name() <<": Done. 'Waiting Reply' loop repeated "
+                  << count << " times" << std::endl;
+        cleanup(false);
+        return NodeStatus::SUCCESS;
+    }
+
+    // you might want to cleanup differently if it was halted or successful
+    void cleanup(bool halted)
+    {
+        if( halted )
+        {
+            std::cout << name() <<": cleaning up after an halt()\n" << std::endl;
+        }
+        else{
+            std::cout << name() <<": cleaning up after SUCCESS\n" << std::endl;
+        }
+    }
+
+    void halt() override
+    {
+        std::cout << name() <<": Halted." << std::endl;
+        cleanup(true);
+        // Do not forget to call this at the end.
+        CoroActionNode::halt();
+    }
+
+    Timepoint Now()
+    { 
+        return std::chrono::high_resolution_clock::now(); 
+    };
+};
+
+```
+
+As you may have noticed, the action "pretends" to wait for a request message;
+the latter will arrive after _100 milliseconds_.
+
+To spice things up, we create a Sequence with two actions, but the entire 
+sequence will be halted by a timeout after _150 millisecond_.
+
+```XML
+ <root >
+     <BehaviorTree>
+        <Timeout msec="150">
+            <SequenceStar name="sequence">
+                <MyAsyncAction name="action_A"/>
+                <MyAsyncAction name="action_B"/>
+            </SequenceStar>
+        </Timeout>
+     </BehaviorTree>
+ </root>
+
+```
+
+No surprises in the `main()`... 
+
+``` c++
+int main()
+{
+    // Simple tree: a sequence of two asycnhronous actions,
+    // but the second will be halted because of the timeout.
+
+    BehaviorTreeFactory factory;
+    factory.registerNodeType<MyAsyncAction>("MyAsyncAction");
+
+    auto tree = factory.createTreeFromText(xml_text);
+
+    //---------------------------------------
+    // keep executin tick until it returns etiher SUCCESS or FAILURE
+    while( tree.tickRoot() == NodeStatus::RUNNING)
+    {
+        std::this_thread::sleep_for( Milliseconds(10) );
+    }
+    return 0;
+}
+
+/* Expected output:
+
+action_A: Started. Send Request to server.
+action_A: Waiting Reply...
+action_A: Done. 'Waiting Reply' loop repeated 11 times
+action_A: cleaning up after SUCCESS
+
+action_B: Started. Send Request to server.
+action_B: Waiting Reply...
+action_B: Halted.
+action_B: cleaning up after an halt()
+
+*/
+```
diff --git a/docs/tutorials_summary.md b/docs/tutorials_summary.md
new file mode 100644 (file)
index 0000000..aa4bade
--- /dev/null
@@ -0,0 +1,64 @@
+# Summary of the tutorials
+
+### T.1: Create your first Behavior Tree
+
+This tutorial demonstrates how to create custom `ActionNodes` in __C++__ and 
+how to compose them into Trees using the __XML__ language.
+
+### T.2: Parametrize a Node with Ports
+
+TreeNodes can have both Inputs and Outputs Ports.
+This tutorial demonstrates how to use ports to create parametrized Nodes.
+
+
+### T.3: Generic and type-safe Ports
+
+This tutorial is an extension of the previous one.
+
+It shows how to create and use ports with generic and user-defined
+types.
+
+### T.4: Difference between Sequence and ReactiveSequence
+
+Reactive ControlNodes can be a very powerful tool to create sophisticated
+behaviors.
+
+This example shows the difference between a standard Sequence and a Reactive one.
+
+### T.5: How to reuse entire SubTrees
+
+Reusability and Composability can be done at the level of a single Node,
+but also with entire Trees, which can become SubTrees of a "parent" Tree.
+
+In this tutorial we will also introduce the builtin Loggers.
+
+### T.6: Remapping of Ports between SubTrees and their parents
+
+Any Tree/SubTree in the system has its own isolated BlackBoard.
+
+In this tutorial we extend the concept or Ports to SubTrees, using 
+port remapping.
+
+### T.7: How to wrap legacy code in a non intrusive way
+
+This tutorial shows one of the many possible ways to wrap an existing code
+into the `BehavioTree.CPP` infrastructure.
+
+### T.8: Passing arguments to Nodes without Ports
+
+If your custom Node has a lot of ports, it is probably a sign that you didn't 
+understand the problem that Ports are supposed to solve ;)
+
+In this tutorial, we show how to pass arguments to a custom Node class without 
+polluting your interfaces with pointless Input Ports.
+
+### T.9: Asynchronous actions with Coroutines
+
+Coroutines are a powerful tool to create asynchronous code.
+
+In this tutorial, we outline the typical design pattern to use when you 
+implement an asynchronous Action using `CoroActionNode`.
+
+
+
+
diff --git a/docs/uml/AllFallbacks.uxf b/docs/uml/AllFallbacks.uxf
new file mode 100644 (file)
index 0000000..b3ee7bb
--- /dev/null
@@ -0,0 +1,181 @@
+<?xml version="1.0" encoding="UTF-8"?><diagram program="umlet" version="13.3">
+  <zoom_level>10</zoom_level>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>260</x>
+      <y>120</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>Fallback
+fg=blue</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>200</x>
+      <y>200</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>OpenDoor</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>250</x>
+      <y>140</y>
+      <w>80</w>
+      <h>80</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>10.0;60.0;60.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>420</x>
+      <y>200</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>SmashDoor</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>300</x>
+      <y>140</y>
+      <w>80</w>
+      <h>80</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>60.0;60.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>300</x>
+      <y>140</y>
+      <w>200</w>
+      <h>80</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>180.0;60.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>310</x>
+      <y>200</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>UnlockDoor</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLUseCase</id>
+    <coordinates>
+      <x>70</x>
+      <y>200</y>
+      <w>120</w>
+      <h>40</h>
+    </coordinates>
+    <panel_attributes>isDoorOpen?</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>120</x>
+      <y>140</y>
+      <w>210</w>
+      <h>80</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>10.0;60.0;190.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>720</x>
+      <y>130</y>
+      <w>160</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>ReactiveFallback
+fg=blue</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLUseCase</id>
+    <coordinates>
+      <x>660</x>
+      <y>200</y>
+      <w>130</w>
+      <h>40</h>
+    </coordinates>
+    <panel_attributes>areYouRested?</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>850</x>
+      <y>270</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>Sleep</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>830</x>
+      <y>200</y>
+      <w>130</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>Timeout (8hrs)</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>710</x>
+      <y>150</y>
+      <w>110</w>
+      <h>70</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>10.0;50.0;90.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>790</x>
+      <y>150</y>
+      <w>120</w>
+      <h>70</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>100.0;50.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>890</x>
+      <y>220</y>
+      <w>30</w>
+      <h>70</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>10.0;10.0;10.0;50.0</additional_attributes>
+  </element>
+</diagram>
diff --git a/docs/uml/AllSequences.uxf b/docs/uml/AllSequences.uxf
new file mode 100644 (file)
index 0000000..0dc50ed
--- /dev/null
@@ -0,0 +1,265 @@
+<?xml version="1.0" encoding="UTF-8"?><diagram program="umlet" version="13.3">
+  <zoom_level>10</zoom_level>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>480</x>
+      <y>150</y>
+      <w>90</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>Shoot</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>350</x>
+      <y>70</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>Sequence
+fg=blue</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>390</x>
+      <y>90</y>
+      <w>150</w>
+      <h>80</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>130.0;60.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLUseCase</id>
+    <coordinates>
+      <x>190</x>
+      <y>150</y>
+      <w>130</w>
+      <h>40</h>
+    </coordinates>
+    <panel_attributes>isEnemyVisible?</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLUseCase</id>
+    <coordinates>
+      <x>330</x>
+      <y>150</y>
+      <w>130</w>
+      <h>40</h>
+    </coordinates>
+    <panel_attributes>isRifleLoaded?</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>240</x>
+      <y>90</y>
+      <w>180</w>
+      <h>80</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>10.0;60.0;160.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>390</x>
+      <y>90</y>
+      <w>30</w>
+      <h>80</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>10.0;60.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>760</x>
+      <y>70</y>
+      <w>150</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>ReactiveSequence
+fg=blue</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLUseCase</id>
+    <coordinates>
+      <x>690</x>
+      <y>140</y>
+      <w>130</w>
+      <h>50</h>
+    </coordinates>
+    <panel_attributes>isEnemyVisible?</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>750</x>
+      <y>90</y>
+      <w>100</w>
+      <h>70</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>10.0;50.0;80.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>850</x>
+      <y>150</y>
+      <w>150</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>ApproachEnemy
+</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>820</x>
+      <y>90</y>
+      <w>120</w>
+      <h>80</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>100.0;60.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>300</x>
+      <y>340</y>
+      <w>170</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>ReactiveSequence
+</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLUseCase</id>
+    <coordinates>
+      <x>250</x>
+      <y>400</y>
+      <w>120</w>
+      <h>40</h>
+    </coordinates>
+    <panel_attributes>isBatteryOK?</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>300</x>
+      <y>360</y>
+      <w>100</w>
+      <h>60</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>10.0;40.0;80.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>370</x>
+      <y>360</y>
+      <w>100</w>
+      <h>60</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>80.0;40.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>410</x>
+      <y>400</y>
+      <w>140</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>SequenceStar
+fg=blue
+</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>360</x>
+      <y>420</y>
+      <w>140</w>
+      <h>70</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>10.0;50.0;120.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>470</x>
+      <y>420</y>
+      <w>30</w>
+      <h>70</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>10.0;50.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>470</x>
+      <y>420</y>
+      <w>140</w>
+      <h>70</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>120.0;50.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>540</x>
+      <y>470</y>
+      <w>100</w>
+      <h>40</h>
+    </coordinates>
+    <panel_attributes>GoTo
+(goal=C")</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>430</x>
+      <y>470</y>
+      <w>100</w>
+      <h>40</h>
+    </coordinates>
+    <panel_attributes>GoTo
+(goal=B")</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>320</x>
+      <y>470</y>
+      <w>100</w>
+      <h>40</h>
+    </coordinates>
+    <panel_attributes>GoTo
+(goal=A")</panel_attributes>
+    <additional_attributes/>
+  </element>
+</diagram>
diff --git a/docs/uml/CrossDoorSubtree.uxf b/docs/uml/CrossDoorSubtree.uxf
new file mode 100644 (file)
index 0000000..8961c8b
--- /dev/null
@@ -0,0 +1,299 @@
+<?xml version="1.0" encoding="UTF-8"?><diagram program="umlet" version="13.3">
+  <zoom_level>10</zoom_level>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>270</x>
+      <y>140</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>Fallback
+</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>270</x>
+      <y>540</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>OpenDoor</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>380</x>
+      <y>220</y>
+      <w>160</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>PassThroughWindow</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>310</x>
+      <y>160</y>
+      <w>150</w>
+      <h>80</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>130.0;60.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>250</x>
+      <y>460</y>
+      <w>150</w>
+      <h>40</h>
+    </coordinates>
+    <panel_attributes>RetryUntilSuccesful
+(num_attempts=4)</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>310</x>
+      <y>110</y>
+      <w>30</w>
+      <h>50</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>10.0;30.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLUseCase</id>
+    <coordinates>
+      <x>70</x>
+      <y>280</y>
+      <w>120</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>isDoorOpen?</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>190</x>
+      <y>160</y>
+      <w>150</w>
+      <h>80</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>10.0;60.0;130.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>310</x>
+      <y>410</y>
+      <w>30</w>
+      <h>70</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>10.0;50.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>190</x>
+      <y>410</y>
+      <w>150</w>
+      <h>80</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>10.0;60.0;130.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>270</x>
+      <y>390</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>Sequence
+</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>160</x>
+      <y>470</y>
+      <w>80</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>Inverter</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLUseCase</id>
+    <coordinates>
+      <x>140</x>
+      <y>530</y>
+      <w>120</w>
+      <h>40</h>
+    </coordinates>
+    <panel_attributes>isDoorOpen?</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>190</x>
+      <y>490</y>
+      <w>30</w>
+      <h>60</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>10.0;40.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>260</x>
+      <y>340</y>
+      <w>120</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>*DoorClosed*
+bg=gray</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>310</x>
+      <y>360</y>
+      <w>30</w>
+      <h>50</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>10.0;30.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>260</x>
+      <y>90</y>
+      <w>120</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>*MainTree*
+bg=gray
+
+</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>310</x>
+      <y>490</y>
+      <w>30</w>
+      <h>70</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>10.0;50.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>410</x>
+      <y>470</y>
+      <w>140</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>PassThroughDoor</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>310</x>
+      <y>410</y>
+      <w>190</w>
+      <h>80</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>170.0;60.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>150</x>
+      <y>220</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>Sequence
+</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>200</x>
+      <y>280</y>
+      <w>140</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>PassThroughDoor</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>120</x>
+      <y>240</y>
+      <w>100</w>
+      <h>60</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>80.0;10.0;10.0;40.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>190</x>
+      <y>240</y>
+      <w>100</w>
+      <h>60</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>10.0;10.0;80.0;40.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>260</x>
+      <y>210</y>
+      <w>110</w>
+      <h>40</h>
+    </coordinates>
+    <panel_attributes>Subtree:
+*DoorClosed*
+fg=blue</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>310</x>
+      <y>160</y>
+      <w>30</w>
+      <h>70</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>10.0;50.0;10.0;10.0</additional_attributes>
+  </element>
+</diagram>
diff --git a/docs/uml/EnterRoom.uxf b/docs/uml/EnterRoom.uxf
new file mode 100644 (file)
index 0000000..74afdf8
--- /dev/null
@@ -0,0 +1,298 @@
+<?xml version="1.0" encoding="UTF-8"?><diagram program="umlet" version="13.3">
+  <zoom_level>10</zoom_level>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>260</x>
+      <y>210</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>OpenDoor</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>390</x>
+      <y>140</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>EnterRoom</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>300</x>
+      <y>90</y>
+      <w>160</w>
+      <h>70</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>140.0;50.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLUseCase</id>
+    <coordinates>
+      <x>110</x>
+      <y>200</y>
+      <w>120</w>
+      <h>40</h>
+    </coordinates>
+    <panel_attributes>isDoorOpen?</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>120</x>
+      <y>140</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>Inverter
+fg=blue</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>160</x>
+      <y>160</y>
+      <w>30</w>
+      <h>60</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>10.0;40.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>260</x>
+      <y>70</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>Sequence
+</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>160</x>
+      <y>90</y>
+      <w>170</w>
+      <h>70</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>10.0;50.0;150.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>300</x>
+      <y>90</y>
+      <w>30</w>
+      <h>70</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>10.0;50.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>230</x>
+      <y>140</y>
+      <w>150</w>
+      <h>40</h>
+    </coordinates>
+    <panel_attributes>Retry
+(num_attempts=3)
+fg=blue</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>300</x>
+      <y>170</y>
+      <w>30</w>
+      <h>60</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>10.0;40.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>770</x>
+      <y>70</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>Sequence
+</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>740</x>
+      <y>270</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>OpenDoor</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>680</x>
+      <y>90</y>
+      <w>160</w>
+      <h>70</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>10.0;50.0;140.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>890</x>
+      <y>140</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>CloseDoor</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>770</x>
+      <y>140</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>EnterRoom</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>810</x>
+      <y>90</y>
+      <w>30</w>
+      <h>70</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>10.0;50.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>810</x>
+      <y>90</y>
+      <w>160</w>
+      <h>70</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>140.0;50.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLUseCase</id>
+    <coordinates>
+      <x>570</x>
+      <y>260</y>
+      <w>120</w>
+      <h>40</h>
+    </coordinates>
+    <panel_attributes>isDoorOpen?</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>580</x>
+      <y>200</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>Inverter
+fg=blue</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>620</x>
+      <y>220</y>
+      <w>30</w>
+      <h>60</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>10.0;40.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>640</x>
+      <y>140</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>Sequence
+</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>620</x>
+      <y>160</y>
+      <w>90</w>
+      <h>60</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>10.0;40.0;70.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>680</x>
+      <y>160</y>
+      <w>100</w>
+      <h>60</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>80.0;40.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>710</x>
+      <y>200</y>
+      <w>160</w>
+      <h>40</h>
+    </coordinates>
+    <panel_attributes>Retry
+(num_attempts=3)
+fg=blue</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>780</x>
+      <y>230</y>
+      <w>30</w>
+      <h>60</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>10.0;40.0;10.0;10.0</additional_attributes>
+  </element>
+</diagram>
diff --git a/docs/uml/FallbackBasic.uxf b/docs/uml/FallbackBasic.uxf
new file mode 100644 (file)
index 0000000..8143437
--- /dev/null
@@ -0,0 +1,216 @@
+<?xml version="1.0" encoding="UTF-8"?><diagram program="umlet" version="13.3">
+  <zoom_level>10</zoom_level>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>260</x>
+      <y>140</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>Fallback
+fg=blue</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>200</x>
+      <y>210</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>OpenDoor</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>250</x>
+      <y>160</y>
+      <w>80</w>
+      <h>70</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>10.0;50.0;60.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>430</x>
+      <y>210</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>SmashDoor</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>300</x>
+      <y>160</y>
+      <w>90</w>
+      <h>70</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>70.0;50.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>300</x>
+      <y>160</y>
+      <w>210</w>
+      <h>70</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>190.0;50.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>320</x>
+      <y>290</y>
+      <w>100</w>
+      <h>40</h>
+    </coordinates>
+    <panel_attributes>UnlockDoor</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>370</x>
+      <y>80</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>Sequence
+</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>470</x>
+      <y>140</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>EnterRoom</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>300</x>
+      <y>100</y>
+      <w>140</w>
+      <h>60</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>10.0;40.0;120.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>410</x>
+      <y>100</y>
+      <w>130</w>
+      <h>60</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>110.0;40.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLUseCase</id>
+    <coordinates>
+      <x>70</x>
+      <y>210</y>
+      <w>120</w>
+      <h>40</h>
+    </coordinates>
+    <panel_attributes>isDoorOpen?</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>120</x>
+      <y>160</y>
+      <w>210</w>
+      <h>70</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>10.0;50.0;190.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>430</x>
+      <y>290</y>
+      <w>100</w>
+      <h>40</h>
+    </coordinates>
+    <panel_attributes>OpenDoor</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>360</x>
+      <y>240</y>
+      <w>30</w>
+      <h>70</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>10.0;50.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>360</x>
+      <y>240</y>
+      <w>140</w>
+      <h>70</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>120.0;50.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>240</x>
+      <y>240</y>
+      <w>150</w>
+      <h>70</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>10.0;50.0;130.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLUseCase</id>
+    <coordinates>
+      <x>190</x>
+      <y>290</y>
+      <w>120</w>
+      <h>40</h>
+    </coordinates>
+    <panel_attributes>HaveKey?</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>320</x>
+      <y>210</y>
+      <w>100</w>
+      <h>40</h>
+    </coordinates>
+    <panel_attributes>Sequence
+("Unlock")
+</panel_attributes>
+    <additional_attributes/>
+  </element>
+</diagram>
diff --git a/docs/uml/FetchBeerFridge.uxf b/docs/uml/FetchBeerFridge.uxf
new file mode 100644 (file)
index 0000000..7c3418e
--- /dev/null
@@ -0,0 +1,592 @@
+<?xml version="1.0" encoding="UTF-8"?><diagram program="umlet" version="13.3">
+  <zoom_level>10</zoom_level>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>550</x>
+      <y>30</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>Sequence
+fg=#009000</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>430</x>
+      <y>110</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>OpenFridge
+fg=#009000
+</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>470</x>
+      <y>50</y>
+      <w>150</w>
+      <h>80</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>10.0;60.0;130.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>670</x>
+      <y>110</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>CloseFridge
+fg=#009000</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>560</x>
+      <y>180</y>
+      <w>90</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>GrabBeer
+fg=#B00000</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>590</x>
+      <y>50</y>
+      <w>30</w>
+      <h>80</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>10.0;60.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>590</x>
+      <y>50</y>
+      <w>150</w>
+      <h>80</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>130.0;60.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>590</x>
+      <y>130</y>
+      <w>30</w>
+      <h>70</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>10.0;50.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>920</x>
+      <y>30</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>Sequence
+fg=#B00000</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>850</x>
+      <y>50</y>
+      <w>140</w>
+      <h>80</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>10.0;60.0;120.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>960</x>
+      <y>50</y>
+      <w>30</w>
+      <h>80</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>10.0;60.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>960</x>
+      <y>50</y>
+      <w>140</w>
+      <h>80</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>120.0;60.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>1030</x>
+      <y>110</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>CloseFridge
+</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>920</x>
+      <y>110</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>Fallback
+fg=#B00000</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>810</x>
+      <y>110</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>OpenFridge
+fg=#009000</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>860</x>
+      <y>180</y>
+      <w>90</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>GrabBeer
+fg=#B00000</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>900</x>
+      <y>130</y>
+      <w>90</w>
+      <h>70</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>10.0;50.0;70.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>1020</x>
+      <y>200</y>
+      <w>30</w>
+      <h>50</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>10.0;30.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>980</x>
+      <y>230</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>CloseFridge
+fg=#009000</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>970</x>
+      <y>180</y>
+      <w>110</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>ForceFailure
+fg=#B00000</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>960</x>
+      <y>130</y>
+      <w>80</w>
+      <h>70</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>60.0;50.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>540</x>
+      <y>110</y>
+      <w>120</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>ForceSuccess
+fg=#009000</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>150</x>
+      <y>30</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>Sequence
+fg=blue</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>40</x>
+      <y>110</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>OpenFridge</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>150</x>
+      <y>110</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>GrabBeer</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>260</x>
+      <y>110</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>CloseFridge</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>80</x>
+      <y>50</y>
+      <w>140</w>
+      <h>80</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>10.0;60.0;120.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>190</x>
+      <y>50</y>
+      <w>30</w>
+      <h>80</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>10.0;60.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>190</x>
+      <y>50</y>
+      <w>150</w>
+      <h>80</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>130.0;60.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>570</x>
+      <y>310</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>Sequence
+fg=#009000</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>450</x>
+      <y>390</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>OpenFridge
+fg=#009000
+</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>490</x>
+      <y>330</y>
+      <w>150</w>
+      <h>80</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>10.0;60.0;130.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>680</x>
+      <y>390</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>CloseFridge
+fg=#009000</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>570</x>
+      <y>460</y>
+      <w>90</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>GrabBeer
+fg=#009000</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>610</x>
+      <y>330</y>
+      <w>30</w>
+      <h>80</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>10.0;60.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>610</x>
+      <y>330</y>
+      <w>140</w>
+      <h>80</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>120.0;60.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>940</x>
+      <y>310</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>Sequence
+fg=#009000</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>870</x>
+      <y>330</y>
+      <w>140</w>
+      <h>80</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>10.0;60.0;120.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>980</x>
+      <y>330</y>
+      <w>30</w>
+      <h>80</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>10.0;60.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>980</x>
+      <y>330</y>
+      <w>140</w>
+      <h>80</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>120.0;60.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>1050</x>
+      <y>390</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>CloseFridge
+fg=#009000</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>940</x>
+      <y>390</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>Fallback
+fg=#009000</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>830</x>
+      <y>390</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>OpenFridge
+fg=#009000</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>890</x>
+      <y>460</y>
+      <w>90</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>GrabBeer
+fg=#009000</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>930</x>
+      <y>410</y>
+      <w>80</w>
+      <h>70</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>10.0;50.0;60.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>1030</x>
+      <y>480</y>
+      <w>30</w>
+      <h>50</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>10.0;30.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>990</x>
+      <y>510</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>CloseFridge
+</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>990</x>
+      <y>460</y>
+      <w>110</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>ForceFailure
+</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>980</x>
+      <y>410</y>
+      <w>70</w>
+      <h>70</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>50.0;50.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>610</x>
+      <y>410</y>
+      <w>30</w>
+      <h>70</h>
+    </coordinates>
+    <panel_attributes>lt=-
+</panel_attributes>
+    <additional_attributes>10.0;50.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>560</x>
+      <y>390</y>
+      <w>110</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>ForceSuccess
+fg=#009000</panel_attributes>
+    <additional_attributes/>
+  </element>
+</diagram>
diff --git a/docs/uml/LeafToComponentCommunication.uxf b/docs/uml/LeafToComponentCommunication.uxf
new file mode 100644 (file)
index 0000000..ca5f610
--- /dev/null
@@ -0,0 +1,109 @@
+<?xml version="1.0" encoding="UTF-8"?><diagram program="umlet" version="13.3">
+  <zoom_level>10</zoom_level>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>620</x>
+      <y>150</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>Sequence
+</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>540</x>
+      <y>230</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>DetectObject</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>580</x>
+      <y>170</y>
+      <w>110</w>
+      <h>80</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>10.0;60.0;90.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>710</x>
+      <y>230</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>GraspObject</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>660</x>
+      <y>170</y>
+      <w>120</w>
+      <h>80</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>100.0;60.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>530</x>
+      <y>330</y>
+      <w>140</w>
+      <h>50</h>
+    </coordinates>
+    <panel_attributes>ObjectRecognition
+Component
+bg=orange</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>690</x>
+      <y>330</y>
+      <w>140</w>
+      <h>50</h>
+    </coordinates>
+    <panel_attributes>Manipulation
+Component
+bg=orange</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>580</x>
+      <y>250</y>
+      <w>30</w>
+      <h>100</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;.&gt;
+</panel_attributes>
+    <additional_attributes>10.0;10.0;10.0;80.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>750</x>
+      <y>250</y>
+      <w>30</w>
+      <h>100</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;.&gt;
+</panel_attributes>
+    <additional_attributes>10.0;10.0;10.0;80.0</additional_attributes>
+  </element>
+</diagram>
diff --git a/docs/uml/Reactive.uxf b/docs/uml/Reactive.uxf
new file mode 100644 (file)
index 0000000..eb60589
--- /dev/null
@@ -0,0 +1,260 @@
+<?xml version="1.0" encoding="UTF-8"?><diagram program="umlet" version="13.3">
+  <zoom_level>10</zoom_level>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>360</x>
+      <y>100</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>Sequence</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>160</x>
+      <y>270</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>PickObject</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>490</x>
+      <y>370</y>
+      <w>140</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>AssembleObject</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>700</x>
+      <y>270</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>PlaceObject</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>360</x>
+      <y>200</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>Parallel</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLUseCase</id>
+    <coordinates>
+      <x>360</x>
+      <y>370</y>
+      <w>120</w>
+      <h>40</h>
+    </coordinates>
+    <panel_attributes>Object
+Assembled</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>410</x>
+      <y>310</y>
+      <w>80</w>
+      <h>80</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>10.0;60.0;60.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>460</x>
+      <y>310</y>
+      <w>120</w>
+      <h>80</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>100.0;60.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLUseCase</id>
+    <coordinates>
+      <x>30</x>
+      <y>270</y>
+      <w>120</w>
+      <h>40</h>
+    </coordinates>
+    <panel_attributes>Object
+Picked</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLUseCase</id>
+    <coordinates>
+      <x>570</x>
+      <y>270</y>
+      <w>120</w>
+      <h>40</h>
+    </coordinates>
+    <panel_attributes>Object
+Placed</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>120</x>
+      <y>200</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>Fallback</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>650</x>
+      <y>200</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>Parallel</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>620</x>
+      <y>220</y>
+      <w>100</w>
+      <h>70</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>10.0;50.0;80.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>690</x>
+      <y>220</y>
+      <w>80</w>
+      <h>70</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>60.0;50.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>80</x>
+      <y>220</y>
+      <w>110</w>
+      <h>70</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>10.0;50.0;90.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>160</x>
+      <y>220</y>
+      <w>70</w>
+      <h>70</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>50.0;50.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>160</x>
+      <y>120</y>
+      <w>270</w>
+      <h>100</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>10.0;80.0;250.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>400</x>
+      <y>120</y>
+      <w>30</w>
+      <h>100</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>10.0;80.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>400</x>
+      <y>120</y>
+      <w>320</w>
+      <h>100</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>300.0;80.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLUseCase</id>
+    <coordinates>
+      <x>290</x>
+      <y>290</y>
+      <w>120</w>
+      <h>40</h>
+    </coordinates>
+    <panel_attributes>Object
+Picked</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>340</x>
+      <y>220</y>
+      <w>90</w>
+      <h>90</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>10.0;70.0;70.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>420</x>
+      <y>290</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>Fallback</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>400</x>
+      <y>220</y>
+      <w>80</w>
+      <h>90</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>60.0;70.0;10.0;10.0</additional_attributes>
+  </element>
+</diagram>
diff --git a/docs/uml/ReadTheDocs.uxf b/docs/uml/ReadTheDocs.uxf
new file mode 100644 (file)
index 0000000..363b8b1
--- /dev/null
@@ -0,0 +1,153 @@
+<?xml version="1.0" encoding="UTF-8"?><diagram program="umlet" version="13.3">
+  <zoom_level>10</zoom_level>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>290</x>
+      <y>100</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>Sequence
+</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>350</x>
+      <y>160</y>
+      <w>160</w>
+      <h>40</h>
+    </coordinates>
+    <panel_attributes>Build Awesome
+Robot Behaviors</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>330</x>
+      <y>120</y>
+      <w>100</w>
+      <h>60</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>80.0;40.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>180</x>
+      <y>160</y>
+      <w>150</w>
+      <h>40</h>
+    </coordinates>
+    <panel_attributes>RetryUntilSuccesful</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>330</x>
+      <y>70</y>
+      <w>30</w>
+      <h>50</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>10.0;30.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLUseCase</id>
+    <coordinates>
+      <x>130</x>
+      <y>280</y>
+      <w>110</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>isItClear?</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>240</x>
+      <y>120</y>
+      <w>120</w>
+      <h>60</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>10.0;40.0;100.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>240</x>
+      <y>190</y>
+      <w>30</w>
+      <h>50</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>10.0;30.0;10.0;10.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>280</x>
+      <y>50</y>
+      <w>120</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>*BehaviorTree*
+bg=gray
+
+</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>200</x>
+      <y>220</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>Fallback
+</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>260</x>
+      <y>280</y>
+      <w>120</w>
+      <h>40</h>
+    </coordinates>
+    <panel_attributes>Read
+Documentation</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>170</x>
+      <y>240</y>
+      <w>100</w>
+      <h>60</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>80.0;10.0;15.0;40.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>240</x>
+      <y>240</y>
+      <w>100</w>
+      <h>60</h>
+    </coordinates>
+    <panel_attributes>lt=-</panel_attributes>
+    <additional_attributes>10.0;10.0;80.0;40.0</additional_attributes>
+  </element>
+</diagram>
diff --git a/docs/uml/TypeHierarchy.uxf b/docs/uml/TypeHierarchy.uxf
new file mode 100644 (file)
index 0000000..34e0059
--- /dev/null
@@ -0,0 +1,141 @@
+<?xml version="1.0" encoding="UTF-8"?><diagram program="umlet" version="13.3">
+  <help_text>This is the type hierachy
+</help_text>
+  <zoom_level>10</zoom_level>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>490</x>
+      <y>180</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>TreeNode</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>590</x>
+      <y>280</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>ControlNode</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>590</x>
+      <y>320</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>LeafNode</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>590</x>
+      <y>240</y>
+      <w>120</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>DecoratorNode</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>500</x>
+      <y>200</y>
+      <w>110</w>
+      <h>160</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;&lt;-</panel_attributes>
+    <additional_attributes>10.0;10.0;10.0;140.0;90.0;140.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>530</x>
+      <y>200</y>
+      <w>80</w>
+      <h>110</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;&lt;-</panel_attributes>
+    <additional_attributes>10.0;10.0;10.0;90.0;60.0;90.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>560</x>
+      <y>200</y>
+      <w>50</w>
+      <h>70</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;&lt;-</panel_attributes>
+    <additional_attributes>10.0;10.0;10.0;50.0;30.0;50.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>700</x>
+      <y>420</y>
+      <w>100</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>ActionNode</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>700</x>
+      <y>380</y>
+      <w>120</w>
+      <h>30</h>
+    </coordinates>
+    <panel_attributes>ConditionNode</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>620</x>
+      <y>340</y>
+      <w>100</w>
+      <h>120</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;&lt;-</panel_attributes>
+    <additional_attributes>10.0;10.0;10.0;100.0;80.0;100.0</additional_attributes>
+  </element>
+  <element>
+    <id>Relation</id>
+    <coordinates>
+      <x>640</x>
+      <y>340</y>
+      <w>80</w>
+      <h>70</h>
+    </coordinates>
+    <panel_attributes>lt=&lt;&lt;-</panel_attributes>
+    <additional_attributes>10.0;10.0;10.0;50.0;60.0;50.0</additional_attributes>
+  </element>
+  <element>
+    <id>UMLNote</id>
+    <coordinates>
+      <x>460</x>
+      <y>360</y>
+      <w>140</w>
+      <h>90</h>
+    </coordinates>
+    <panel_attributes>Note..
+
+This is the type 
+hierarchy in UML
+bg=cyan</panel_attributes>
+    <additional_attributes/>
+  </element>
+</diagram>
diff --git a/docs/video_MOOD2Be.png b/docs/video_MOOD2Be.png
new file mode 100644 (file)
index 0000000..f44586a
Binary files /dev/null and b/docs/video_MOOD2Be.png differ
diff --git a/docs/xml_format.md b/docs/xml_format.md
new file mode 100644 (file)
index 0000000..d4ff9fc
--- /dev/null
@@ -0,0 +1,221 @@
+
+## Basics of the XML schema
+
+In the [first tutorial](tutorial_01_first_tree.md) this simple tree
+was presented.
+
+``` XML
+ <root main_tree_to_execute = "MainTree" >
+     <BehaviorTree ID="MainTree">
+        <Sequence name="root_sequence">
+            <SaySomething   name="action_hello" message="Hello"/>
+            <OpenGripper    name="open_gripper"/>
+            <ApproachObject name="approach_object"/>
+            <CloseGripper   name="close_gripper"/>
+        </Sequence>
+     </BehaviorTree>
+ </root>
+```
+
+You may notice that:
+
+- The first tag of the tree is `<root>`. It should contain __1 or more__ tags `<BehaviorTree>`.
+
+- The tag `<BehaviorTree>` should have the attribute `[ID]`.
+
+- The tag `<root>` should contain the attribute `[main_tree_to_execute]`.
+
+- The attribute `[main_tree_to_execute]` is mandatory if the file contains multiple `<BehaviorTree>`, 
+  optional otherwise.
+
+- Each TreeNode is represented by a single tag. In particular:
+
+     - The name of the tag is the __ID__ used to register the TreeNode in the factory.
+     - The attribute `[name]` refers to the name of the instance and is __optional__.
+     - Ports are configured using attributes. In the previous example, the action 
+     `SaySomething` requires the input port `message`.
+
+- In terms of number of children:
+
+     - `ControlNodes` contain __1 to N children__.
+     - `DecoratorNodes` and Subtrees contain __only 1 child__.
+     - `ActionNodes` and `ConditionNodes` have __no child__. 
+
+## Ports Remapping and pointers to Blackboards entries
+
+As explained in the [second tutorial](tutorial_02_basic_ports.md)
+input/output ports can be remapped using the name of an entry in the
+Blackboard, in other words, the __key__ of a __key/value__ pair of the BB.
+
+An BB key is represented using this syntax: `{key_name}`.
+
+In the following example:
+
+- the first child of the Sequence prints "Hello",
+- the second child reads and writes the value contained in the entry of 
+  the blackboard called "my_message"; 
+
+``` XML
+ <root main_tree_to_execute = "MainTree" >
+     <BehaviorTree ID="MainTree">
+        <Sequence name="root_sequence">
+            <SaySomething message="Hello"/>
+            <SaySomething message="{my_message}"/>
+        </Sequence>
+     </BehaviorTree>
+ </root>
+```
+     
+
+## Compact vs Explicit representation
+
+The following two syntaxes are both valid:
+
+``` XML
+ <SaySomething               name="action_hello" message="Hello World"/>
+ <Action ID="SaySomething"   name="action_hello" message="Hello World"/>
+```
+
+We will call the former syntax "__compact__" and the latter "__explicit__".
+The first example represented with the explicit syntax would become:
+
+``` XML
+ <root main_tree_to_execute = "MainTree" >
+     <BehaviorTree ID="MainTree">
+        <Sequence name="root_sequence">
+           <Action ID="SaySomething"   name="action_hello" message="Hello"/>
+           <Action ID="OpenGripper"    name="open_gripper"/>
+           <Action ID="ApproachObject" name="approach_object"/>
+           <Action ID="CloseGripper"   name="close_gripper"/>
+        </Sequence>
+     </BehaviorTree>
+ </root>
+```
+
+Even if the compact syntax is more convenient and easier to write, it provides 
+too little information about the model of the TreeNode. Tools like __Groot__ require either
+the _explicit_ syntax or additional information.
+This information can be added using the tag `<TreeNodeModel>`.
+
+To make the compact version of our tree compatible with Groot, the XML 
+must be modified as follows:
+
+
+``` XML
+ <root main_tree_to_execute = "MainTree" >
+     <BehaviorTree ID="MainTree">
+        <Sequence name="root_sequence">
+           <SaySomething   name="action_hello" message="Hello"/>
+           <OpenGripper    name="open_gripper"/>
+           <ApproachObject name="approach_object"/>
+           <CloseGripper   name="close_gripper"/>
+        </Sequence>
+    </BehaviorTree>
+       
+       <!-- the BT executor don't require this, but Groot does -->     
+    <TreeNodeModel>
+        <Action ID="SaySomething">
+            <input_port name="message" type="std::string" />
+        </Action>
+        <Action ID="OpenGripper"/>
+        <Action ID="ApproachObject"/>
+        <Action ID="CloseGripper"/>      
+    </TreeNodeModel>
+ </root>
+```
+
+!!! Note "XML Schema available for explicit version"
+    You can download the [XML Schema](https://www.w3schools.com/xml/schema_intro.asp) here:
+    [behaviortree_schema.xsd](https://github.com/BehaviorTree/BehaviorTree.CPP/blob/master/behaviortree_schema.xsd).
+
+## Subtrees
+
+As we saw in [this tutorial](tutorial_06_subtree_ports.md), it is possible to include
+a Subtree inside another tree to avoid "copy and pasting" the same tree in
+multiple location and to reduce complexity.
+
+Let's say that we want to incapsulate few action into the behaviorTree "__GraspObject__" 
+(being optional, attributes [name] are omitted for simplicity).
+
+``` XML  hl_lines="6"
+ <root main_tree_to_execute = "MainTree" >
+     <BehaviorTree ID="MainTree">
+        <Sequence>
+           <Action  ID="SaySomething"  message="Hello World"/>
+           <Subtree ID="GraspObject"/>
+        </Sequence>
+     </BehaviorTree>
+     
+     <BehaviorTree ID="GraspObject">
+        <Sequence>
+           <Action ID="OpenGripper"/>
+           <Action ID="ApproachObject"/>
+           <Action ID="CloseGripper"/>
+        </Sequence>
+     </BehaviorTree>  
+ </root>
+```
+
+We may notice as the entire tree "GraspObject" is executed after "SaySomething".
+
+## Include external files
+
+__Since version 2.4__.
+
+You can include external files in a way that is similar to __#include <file>__ in C++.
+We can do this easily using the tag:
+
+``` XML
+  <include path="relative_or_absolute_path_to_file">
+``` 
+
+using the previous example, we may split the two behavior trees into two files:
+
+
+``` XML hl_lines="5"
+ <!-- file maintree.xml -->
+
+ <root main_tree_to_execute = "MainTree" >
+        
+        <include path="grasp.xml"/>
+        
+     <BehaviorTree ID="MainTree">
+        <Sequence>
+           <Action  ID="SaySomething"  message="Hello World"/>
+           <Subtree ID="GraspObject"/>
+        </Sequence>
+     </BehaviorTree>
+  </root>
+``` 
+
+``` XML
+ <!-- file grasp.xml -->
+
+ <root main_tree_to_execute = "GraspObject" >
+     <BehaviorTree ID="GraspObject">
+        <Sequence>
+           <Action ID="OpenGripper"/>
+           <Action ID="ApproachObject"/>
+           <Action ID="CloseGripper"/>
+        </Sequence>
+     </BehaviorTree>  
+ </root>
+```
+
+!!! Note "Note for ROS users"
+    If you want to find a file inside a [ROS package](http://wiki.ros.org/Packages), 
+    you can use this syntax:
+    
+    `<include ros_pkg="name_package"  path="path_relative_to_pkg/grasp.xml"/>`
+
+
+
+
+
+
+
+
+       
+
diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt
new file mode 100644 (file)
index 0000000..2ba4fa2
--- /dev/null
@@ -0,0 +1,53 @@
+cmake_minimum_required(VERSION 3.5.1)
+
+include_directories( ../sample_nodes )
+
+set(CMAKE_DEBUG_POSTFIX "")
+
+# The plugin libdummy_nodes.so can be linked statically or
+# loaded dynamically at run-time.
+add_executable(t01_first_tree_static  t01_build_your_first_tree.cpp )
+target_compile_definitions(t01_first_tree_static PRIVATE "MANUAL_STATIC_LINKING")
+target_link_libraries(t01_first_tree_static ${BEHAVIOR_TREE_LIBRARY}   bt_sample_nodes )
+
+add_executable(t01_first_tree_dynamic  t01_build_your_first_tree.cpp )
+target_link_libraries(t01_first_tree_dynamic ${BEHAVIOR_TREE_LIBRARY}  )
+
+
+add_executable(t02_basic_ports  t02_basic_ports.cpp )
+target_link_libraries(t02_basic_ports ${BEHAVIOR_TREE_LIBRARY} bt_sample_nodes )
+
+add_executable(t03_generic_ports        t03_generic_ports.cpp )
+target_link_libraries(t03_generic_ports ${BEHAVIOR_TREE_LIBRARY}  bt_sample_nodes  )
+
+add_executable(t04_reactive_sequence         t04_reactive_sequence.cpp )
+target_link_libraries(t04_reactive_sequence  ${BEHAVIOR_TREE_LIBRARY} bt_sample_nodes  )
+
+add_executable(t05_cross_door         t05_crossdoor.cpp )
+target_link_libraries(t05_cross_door   ${BEHAVIOR_TREE_LIBRARY} bt_sample_nodes )
+
+add_executable(t06_subtree_port_remapping  t06_subtree_port_remapping.cpp )
+target_link_libraries(t06_subtree_port_remapping ${BEHAVIOR_TREE_LIBRARY}   bt_sample_nodes )
+
+add_executable(t07_wrap_legacy         t07_wrap_legacy.cpp )
+target_link_libraries(t07_wrap_legacy   ${BEHAVIOR_TREE_LIBRARY} )
+
+add_executable(t08_additional_node_args   t08_additional_node_args.cpp )
+target_link_libraries(t08_additional_node_args   ${BEHAVIOR_TREE_LIBRARY} )
+
+if (BT_COROUTINES)
+    add_executable(t09_async_actions_coroutines  t09_async_actions_coroutines.cpp )
+    target_link_libraries(t09_async_actions_coroutines ${BEHAVIOR_TREE_LIBRARY} )
+endif()
+
+add_executable(t10_include_trees  t10_include_trees.cpp )
+target_link_libraries(t10_include_trees  ${BEHAVIOR_TREE_LIBRARY} bt_sample_nodes )
+
+
+add_executable(t11_runtime_ports  t11_runtime_ports.cpp )
+target_link_libraries(t11_runtime_ports  ${BEHAVIOR_TREE_LIBRARY} bt_sample_nodes )
+
+if(CURSES_FOUND)
+    add_executable(t12_ncurses_manual_selector  t12_ncurses_manual_selector.cpp )
+    target_link_libraries(t12_ncurses_manual_selector  ${BEHAVIOR_TREE_LIBRARY} bt_sample_nodes )
+endif()
diff --git a/examples/broken_sequence.cpp b/examples/broken_sequence.cpp
new file mode 100644 (file)
index 0000000..ed0c223
--- /dev/null
@@ -0,0 +1,86 @@
+#include "Blackboard/blackboard_local.h"
+#include "behaviortree_cpp_v3/behavior_tree.h"
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+using namespace BT;
+
+NodeStatus SayHello()
+{
+    printf("hello\n");
+    return NodeStatus::SUCCESS;
+}
+
+class ActionTestNode : public ActionNode
+{
+  public:
+    ActionTestNode(const std::string& name) : ActionNode(name)
+    {
+    }
+
+    NodeStatus tick() override
+    {
+        time_ = 5;
+        stop_loop_ = false;
+        int i = 0;
+        while (!stop_loop_ && i++ < time_)
+        {
+            std::this_thread::sleep_for(std::chrono::milliseconds(100));
+        }
+        return NodeStatus::SUCCESS;
+    }
+
+    virtual void halt() override
+    {
+        stop_loop_ = true;
+        setStatus(NodeStatus::IDLE);
+    }
+
+  private:
+    int time_;
+    std::atomic_bool stop_loop_;
+};
+
+int main()
+{
+    BT::SequenceNode root("root");
+    BT::SimpleActionNode action1("say_hello", std::bind(SayHello));
+    ActionTestNode action2("async_action");
+
+    root.addChild(&action1);
+    root.addChild(&action2);
+
+    int count = 0;
+
+    NodeStatus status = NodeStatus::RUNNING;
+
+    while (status == NodeStatus::RUNNING)
+    {
+        status = root.executeTick();
+
+        std::cout << count++ << " : " << root.status() << " / " << action1.status() << " / "
+                  << action2.status() << std::endl;
+
+        std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    }
+
+    
+
+    return 0;
+}
+// Output
+/*
+
+hello
+0 : RUNNING / SUCCESS / RUNNING
+hello
+1 : RUNNING / SUCCESS / RUNNING
+hello
+2 : RUNNING / SUCCESS / RUNNING
+hello
+3 : RUNNING / SUCCESS / RUNNING
+hello
+4 : RUNNING / SUCCESS / RUNNING
+hello
+5 : SUCCESS / IDLE / IDLE
+
+*/
diff --git a/examples/t01_build_your_first_tree.cpp b/examples/t01_build_your_first_tree.cpp
new file mode 100644 (file)
index 0000000..4ffe0c6
--- /dev/null
@@ -0,0 +1,95 @@
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+//#define MANUAL_STATIC_LINKING
+
+#ifdef MANUAL_STATIC_LINKING
+#include "dummy_nodes.h"
+#endif
+
+using namespace BT;
+
+/** Behavior Tree are used to create a logic to decide what
+ * to "do" and when. For this reason, our main building blocks are
+ * Actions and Conditions.
+ *
+ * In this tutorial, we will learn how to create custom ActionNodes.
+ * It is important to remember that NodeTree are just a way to
+ * invoke callbacks (called tick() ). These callbacks are implemented by the user.
+ */
+
+// clang-format off
+static const char* xml_text = R"(
+
+ <root main_tree_to_execute = "MainTree" >
+
+     <BehaviorTree ID="MainTree">
+        <Sequence name="root_sequence">
+            <CheckBattery   name="battery_ok"/>
+            <OpenGripper    name="open_gripper"/>
+            <ApproachObject name="approach_object"/>
+            <CloseGripper   name="close_gripper"/>
+        </Sequence>
+     </BehaviorTree>
+
+ </root>
+ )";
+
+// clang-format on
+
+int main()
+{
+    // We use the BehaviorTreeFactory to register our custom nodes
+    BehaviorTreeFactory factory;
+
+    /* There are two ways to register nodes:
+    *    - statically, i.e. registering all the nodes one by one.
+    *    - dynamically, loading the TreeNodes from a shared library (plugin).
+    * */
+
+#ifdef MANUAL_STATIC_LINKING
+    // Note: the name used to register should be the same used in the XML.
+    // Note that the same operations could be done using DummyNodes::RegisterNodes(factory)
+
+    using namespace DummyNodes;
+
+    // The recommended way to create a Node is through inheritance.
+    // Even if it requires more boilerplate, it allows you to use more functionalities
+    // like ports (we will discuss this in future tutorials).
+    factory.registerNodeType<ApproachObject>("ApproachObject");
+
+    // Registering a SimpleActionNode using a function pointer.
+    // you may also use C++11 lambdas instead of std::bind
+    factory.registerSimpleCondition("CheckBattery", std::bind(CheckBattery));
+
+    //You can also create SimpleActionNodes using methods of a class
+    GripperInterface gripper;
+    factory.registerSimpleAction("OpenGripper", std::bind(&GripperInterface::open, &gripper));
+    factory.registerSimpleAction("CloseGripper", std::bind(&GripperInterface::close, &gripper));
+
+#else
+    // Load dynamically a plugin and register the TreeNodes it contains
+    // it automated the registering step.
+    factory.registerFromPlugin("./libdummy_nodes_dyn.so");
+#endif
+
+    // Trees are created at deployment-time (i.e. at run-time, but only once at the beginning).
+    // The currently supported format is XML.
+    // IMPORTANT: when the object "tree" goes out of scope, all the TreeNodes are destroyed
+    auto tree = factory.createTreeFromText(xml_text);
+
+    // To "execute" a Tree you need to "tick" it.
+    // The tick is propagated to the children based on the logic of the tree.
+    // In this case, the entire sequence is executed, because all the children
+    // of the Sequence return SUCCESS.
+    tree.tickRoot();
+
+    return 0;
+}
+
+/* Expected output:
+*
+       [ Battery: OK ]
+       GripperInterface::open
+       ApproachObject: approach_object
+       GripperInterface::close
+*/
diff --git a/examples/t02_basic_ports.cpp b/examples/t02_basic_ports.cpp
new file mode 100644 (file)
index 0000000..8e5a7c8
--- /dev/null
@@ -0,0 +1,123 @@
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+#include "dummy_nodes.h"
+#include "movebase_node.h"
+
+using namespace BT;
+
+/** This tutorial will teach you how basic input/output ports work.
+ *
+ * Ports are a mechanism to exchange information between Nodes using
+ * a key/value storage called "Blackboard".
+ * The type and number of ports of a Node is statically defined.
+ *
+ * Input Ports are like "argument" of a functions.
+ * Output ports are, conceptually, like "return values".
+ *
+ * In this example, a Sequence of 5 Actions is executed:
+ *
+ *   - Actions 1 and 4 read the input "message" from a static string.
+ *
+ *   - Actions 3 and 5 read the input "message" from an entry in the
+ *     blackboard called "the_answer".
+ *
+ *   - Action 2 writes something into the entry of the blackboard
+ *     called "the_answer".
+*/
+
+// clang-format off
+static const char* xml_text = R"(
+
+ <root main_tree_to_execute = "MainTree" >
+
+     <BehaviorTree ID="MainTree">
+        <Sequence name="root">
+            <SaySomething     message="start thinking..." />
+            <ThinkWhatToSay   text="{the_answer}"/>
+            <SaySomething     message="{the_answer}" />
+            <SaySomething2    message="SaySomething2 works too..." />
+            <SaySomething2    message="{the_answer}" />
+        </Sequence>
+     </BehaviorTree>
+
+ </root>
+ )";
+// clang-format on
+
+
+class ThinkWhatToSay : public BT::SyncActionNode
+{
+  public:
+    ThinkWhatToSay(const std::string& name, const BT::NodeConfiguration& config)
+      : BT::SyncActionNode(name, config)
+    {
+    }
+
+    // This Action simply write a value in the port "text"
+    BT::NodeStatus tick() override
+    {
+        setOutput("text", "The answer is 42" );
+        return BT::NodeStatus::SUCCESS;
+    }
+
+    // A node having ports MUST implement this STATIC method
+    static BT::PortsList providedPorts()
+    {
+        return { BT::OutputPort<std::string>("text") };
+    }
+};
+
+
+int main()
+{
+    using namespace DummyNodes;
+
+    BehaviorTreeFactory factory;
+
+    // The class SaySomething has a method called providedPorts() that define the INPUTS.
+    // In this case, it requires an input called "message"
+    factory.registerNodeType<SaySomething>("SaySomething");
+
+
+    // Similarly to SaySomething, ThinkWhatToSay has an OUTPUT port called "text"
+    // Both these ports are std::string, therefore they can connect to each other
+    factory.registerNodeType<ThinkWhatToSay>("ThinkWhatToSay");
+
+    // SimpleActionNodes can not define their own method providedPorts(), therefore
+    // we have to pass the PortsList explicitly if we want the Action to use getInput()
+    // or setOutput();
+    PortsList say_something_ports = { InputPort<std::string>("message") };
+    factory.registerSimpleAction("SaySomething2", SaySomethingSimple, say_something_ports );
+
+    /* An INPUT can be either a string, for instance:
+     *
+     *     <SaySomething message="start thinking..." />
+     *
+     * or contain a "pointer" to a type erased entry in the Blackboard,
+     * using this syntax: {name_of_entry}. Example:
+     *
+     *     <SaySomething message="{the_answer}" />
+     */
+
+    auto tree = factory.createTreeFromText(xml_text);
+
+    tree.tickRoot();
+
+    /*  Expected output:
+     *
+        Robot says: start thinking...
+        Robot says: The answer is 42
+        Robot says: SaySomething2 works too...
+        Robot says: The answer is 42
+    *
+    * The way we "connect" output ports to input ports is to "point" to the same
+    * Blackboard entry.
+    *
+    * This means that ThinkSomething will write into the entry with key "the_answer";
+    * SaySomething and SaySomething2 will read the message from the same entry.
+    *
+    */
+    return 0;
+}
+
+
diff --git a/examples/t03_generic_ports.cpp b/examples/t03_generic_ports.cpp
new file mode 100644 (file)
index 0000000..c02c858
--- /dev/null
@@ -0,0 +1,137 @@
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+using namespace BT;
+
+/* This tutorial will teach you how to deal with ports when its
+ *  type is not std::string.
+*/
+
+
+// We want to be able to use this custom type
+struct Position2D { double x,y; };
+
+// It is recommended (or, in some cases, mandatory) to define a template
+// specialization of convertFromString that converts a string to Position2D.
+namespace BT
+{
+template <> inline Position2D convertFromString(StringView str)
+{
+    printf("Converting string: \"%s\"\n", str.data() );
+
+    // real numbers separated by semicolons
+    auto parts = splitString(str, ';');
+    if (parts.size() != 2)
+    {
+        throw RuntimeError("invalid input)");
+    }
+    else{
+        Position2D output;
+        output.x     = convertFromString<double>(parts[0]);
+        output.y     = convertFromString<double>(parts[1]);
+        return output;
+    }
+}
+} // end namespace BT
+
+
+class CalculateGoal: public SyncActionNode
+{
+public:
+    CalculateGoal(const std::string& name, const NodeConfiguration& config):
+        SyncActionNode(name,config)
+    {}
+
+    NodeStatus tick() override
+    {
+        Position2D mygoal = {1.1, 2.3};
+        setOutput("goal", mygoal);
+        return NodeStatus::SUCCESS;
+    }
+    static PortsList providedPorts()
+    {
+        return { OutputPort<Position2D>("goal") };
+    }
+};
+
+
+class PrintTarget: public SyncActionNode
+{
+public:
+    PrintTarget(const std::string& name, const NodeConfiguration& config):
+        SyncActionNode(name,config)
+    {}
+
+    NodeStatus tick() override
+    {
+        auto res = getInput<Position2D>("target");
+        if( !res )
+        {
+            throw RuntimeError("error reading port [target]:", res.error() );
+        }
+        Position2D goal = res.value();
+        printf("Target positions: [ %.1f, %.1f ]\n", goal.x, goal.y );
+        return NodeStatus::SUCCESS;
+    }
+
+    static PortsList providedPorts()
+    {
+        // Optionally, a port can have a human readable description
+        const char*  description = "Simply print the target on console...";
+        return { InputPort<Position2D>("target", description) };
+    }
+};
+
+//----------------------------------------------------------------
+
+/** The tree is a Sequence of 4 actions
+
+*  1) Store a value of Position2D in the entry "GoalPosition"
+*     using the action CalculateGoal.
+*
+*  2) Call PrintTarget. The input "target" will be read from the Blackboard
+*     entry "GoalPosition".
+*
+*  3) Use the built-in action SetBlackboard to write the key "OtherGoal".
+*     A conversion from string to Position2D will be done under the hood.
+*
+*  4) Call PrintTarget. The input "goal" will be read from the Blackboard
+*     entry "OtherGoal".
+*/
+
+// clang-format off
+static const char* xml_text = R"(
+
+ <root main_tree_to_execute = "MainTree" >
+     <BehaviorTree ID="MainTree">
+        <Sequence name="root">
+            <CalculateGoal   goal="{GoalPosition}" />
+            <PrintTarget     target="{GoalPosition}" />
+            <SetBlackboard   output_key="OtherGoal" value="-1;3" />
+            <PrintTarget     target="{OtherGoal}" />
+        </Sequence>
+     </BehaviorTree>
+ </root>
+ )";
+
+// clang-format on
+
+int main()
+{
+    using namespace BT;
+
+    BehaviorTreeFactory factory;
+    factory.registerNodeType<CalculateGoal>("CalculateGoal");
+    factory.registerNodeType<PrintTarget>("PrintTarget");
+
+    auto tree = factory.createTreeFromText(xml_text);
+    tree.tickRoot();
+
+/* Expected output:
+ *
+    Target positions: [ 1.1, 2.3 ]
+    Converting string: "-1;3"
+    Target positions: [ -1.0, 3.0 ]
+*/
+    return 0;
+}
+
diff --git a/examples/t04_reactive_sequence.cpp b/examples/t04_reactive_sequence.cpp
new file mode 100644 (file)
index 0000000..cec330a
--- /dev/null
@@ -0,0 +1,135 @@
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+#include "dummy_nodes.h"
+#include "movebase_node.h"
+
+using namespace BT;
+
+/** This tutorial will teach you:
+ *
+ *  - The difference between Sequence and SequenceStar
+ *
+ *  - How to create an asynchronous ActionNode (an action that is execute in
+ *    its own thread).
+*/
+
+// clang-format off
+
+static const char* xml_text_sequence = R"(
+
+ <root main_tree_to_execute = "MainTree" >
+
+     <BehaviorTree ID="MainTree">
+        <Sequence name="root">
+            <BatteryOK/>
+            <SaySomething   message="mission started..." />
+            <MoveBase       goal="1;2;3"/>
+            <SaySomething   message="mission completed!" />
+        </Sequence>
+     </BehaviorTree>
+
+ </root>
+ )";
+
+static const char* xml_text_reactive = R"(
+
+ <root main_tree_to_execute = "MainTree" >
+
+     <BehaviorTree ID="MainTree">
+        <ReactiveSequence name="root">
+            <BatteryOK/>
+            <Sequence>
+                <SaySomething   message="mission started..." />
+                <MoveBase       goal="1;2;3"/>
+                <SaySomething   message="mission completed!" />
+            </Sequence>
+        </ReactiveSequence>
+     </BehaviorTree>
+
+ </root>
+ )";
+
+// clang-format on
+
+void Assert(bool condition)
+{
+    if (!condition)
+        throw RuntimeError("this is not what I expected");
+}
+
+int main()
+{
+    using namespace DummyNodes;
+
+    BehaviorTreeFactory factory;
+    factory.registerSimpleCondition("BatteryOK", std::bind(CheckBattery));
+    factory.registerNodeType<MoveBaseAction>("MoveBase");
+    factory.registerNodeType<SaySomething>("SaySomething");
+
+    // Compare the state transitions and messages using either
+    // xml_text_sequence and xml_text_sequence_star
+
+    // The main difference that you should notice is:
+    //  1) When Sequence is used, BatteryOK is executed at __each__ tick()
+    //  2) When SequenceStar is used, those ConditionNodes are executed only __once__.
+
+    for (auto& xml_text : {xml_text_sequence, xml_text_reactive})
+    {
+        std::cout << "\n------------ BUILDING A NEW TREE ------------" << std::endl;
+
+        auto tree = factory.createTreeFromText(xml_text);
+
+        NodeStatus status;
+
+        std::cout << "\n--- 1st executeTick() ---" << std::endl;
+        status = tree.tickRoot();
+        Assert(status == NodeStatus::RUNNING);
+
+        SleepMS(150);
+        std::cout << "\n--- 2nd executeTick() ---" << std::endl;
+        status = tree.tickRoot();
+        Assert(status == NodeStatus::RUNNING);
+
+        SleepMS(150);
+        std::cout << "\n--- 3rd executeTick() ---" << std::endl;
+        status = tree.tickRoot();
+        Assert(status == NodeStatus::SUCCESS);
+
+        std::cout << std::endl;
+    }
+    return 0;
+}
+
+/*
+ Expected output:
+
+------------ BUILDING A NEW TREE ------------
+
+--- 1st executeTick() ---
+[ Battery: OK ]
+Robot says: "mission started..."
+[ MoveBase: STARTED ]. goal: x=1 y=2.0 theta=3.00
+
+--- 2nd executeTick() ---
+[ Battery: OK ]
+[ MoveBase: FINISHED ]
+
+--- 3rd executeTick() ---
+[ Battery: OK ]
+Robot says: "mission completed!"
+
+
+------------ BUILDING A NEW TREE ------------
+
+--- 1st executeTick() ---
+[ Battery: OK ]
+Robot says: "mission started..."
+[ MoveBase: STARTED ]. goal: x=1 y=2.0 theta=3.00
+
+--- 2nd executeTick() ---
+[ MoveBase: FINISHED ]
+
+--- 3rd executeTick() ---
+Robot says: "mission completed!"
+
+*/
diff --git a/examples/t05_crossdoor.cpp b/examples/t05_crossdoor.cpp
new file mode 100644 (file)
index 0000000..53de346
--- /dev/null
@@ -0,0 +1,102 @@
+#include "crossdoor_nodes.h"
+
+#include "behaviortree_cpp_v3/loggers/bt_cout_logger.h"
+#include "behaviortree_cpp_v3/loggers/bt_minitrace_logger.h"
+#include "behaviortree_cpp_v3/loggers/bt_file_logger.h"
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+#ifdef ZMQ_FOUND
+#include "behaviortree_cpp_v3/loggers/bt_zmq_publisher.h"
+#endif
+
+/** This is a more complex example that uses Fallback,
+ * Decorators and Subtrees
+ *
+ * For the sake of simplicity, we aren't focusing on ports remapping to the time being.
+ *
+ * Furthermore, we introduce Loggers, which are a mechanism to
+ * trace the state transitions in the tree for debugging purposes.
+ */
+
+
+// clang-format off
+
+static const char* xml_text = R"(
+<root main_tree_to_execute = "MainTree">
+       <!--------------------------------------->
+    <BehaviorTree ID="DoorClosed">
+        <Sequence name="door_closed_sequence">
+            <Inverter>
+                <Condition ID="IsDoorOpen"/>
+            </Inverter>
+            <RetryUntilSuccesful num_attempts="4">
+                <OpenDoor/>
+            </RetryUntilSuccesful>
+            <PassThroughDoor/>
+        </Sequence>
+    </BehaviorTree>
+    <!--------------------------------------->
+    <BehaviorTree ID="MainTree">
+        <Sequence>
+            <Fallback name="root_Fallback">
+                <Sequence name="door_open_sequence">
+                    <IsDoorOpen/>
+                    <PassThroughDoor/>
+                </Sequence>
+                <SubTree ID="DoorClosed"/>
+                <PassThroughWindow/>
+            </Fallback>
+            <CloseDoor/>
+        </Sequence>
+    </BehaviorTree>
+    <!---------------------------------------> 
+</root>
+ )";
+
+// clang-format on
+
+using namespace BT;
+
+int main(int argc, char** argv)
+{
+    BT::BehaviorTreeFactory factory;
+
+    // register all the actions into the factory
+    CrossDoor::RegisterNodes(factory);
+
+    // Important: when the object tree goes out of scope, all the TreeNodes are destroyed
+    auto tree = factory.createTreeFromText(xml_text);
+
+    // This logger prints state changes on console
+    StdCoutLogger logger_cout(tree);
+
+    // This logger saves state changes on file
+    FileLogger logger_file(tree, "bt_trace.fbl");
+
+    // This logger stores the execution time of each node
+    MinitraceLogger logger_minitrace(tree, "bt_trace.json");
+
+#ifdef ZMQ_FOUND
+    // This logger publish status changes using ZeroMQ. Used by Groot
+    PublisherZMQ publisher_zmq(tree);
+#endif
+
+    printTreeRecursively(tree.rootNode());
+
+    const bool LOOP = ( argc == 2 && strcmp( argv[1], "loop") == 0);
+
+    do
+    {
+        NodeStatus status = NodeStatus::RUNNING;
+        // Keep on ticking until you get either a SUCCESS or FAILURE state
+        while( status == NodeStatus::RUNNING)
+        {
+            status = tree.tickRoot();
+            CrossDoor::SleepMS(1);   // optional sleep to avoid "busy loops"
+        }
+        CrossDoor::SleepMS(1000);
+    }
+    while(LOOP);
+
+    return 0;
+}
diff --git a/examples/t06_subtree_port_remapping.cpp b/examples/t06_subtree_port_remapping.cpp
new file mode 100644 (file)
index 0000000..cc0304b
--- /dev/null
@@ -0,0 +1,97 @@
+#include "behaviortree_cpp_v3/loggers/bt_cout_logger.h"
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+#include "movebase_node.h"
+#include "dummy_nodes.h"
+
+/** In the CrossDoor example we did not exchange any information
+ * between the Maintree and the DoorClosed subtree.
+ *
+ * If we tried to do that, we would have noticed that it can't be done, because
+ * each of the tree/subtree has its own Blackboard, to avoid the problem of name
+ * clashing in very large trees.
+ *
+ * But a SubTree can have its own input/output ports.
+ * In practice, these ports are nothing more than "soft links" between the
+ * ports inside the SubTree (called "internal") and those in the parent
+ * Tree (called "external").
+ *
+ */
+
+// clang-format off
+
+static const char* xml_text = R"(
+<root main_tree_to_execute = "MainTree">
+
+    <BehaviorTree ID="MainTree">
+
+        <Sequence name="main_sequence">
+            <SetBlackboard output_key="move_goal" value="1;2;3" />
+            <SubTree ID="MoveRobot" target="move_goal" output="move_result" />
+            <SaySomething message="{move_result}"/>
+        </Sequence>
+
+    </BehaviorTree>
+
+    <BehaviorTree ID="MoveRobot">
+        <Fallback name="move_robot_main">
+            <SequenceStar>
+                <MoveBase       goal="{target}"/>
+                <SetBlackboard output_key="output" value="mission accomplished" />
+            </SequenceStar>
+            <ForceFailure>
+                <SetBlackboard output_key="output" value="mission failed" />
+            </ForceFailure>
+        </Fallback>
+    </BehaviorTree>
+
+</root>
+ )";
+
+// clang-format on
+
+
+using namespace BT;
+using namespace DummyNodes;
+
+int main()
+{
+    BehaviorTreeFactory factory;
+
+    factory.registerNodeType<SaySomething>("SaySomething");
+    factory.registerNodeType<MoveBaseAction>("MoveBase");
+
+    auto tree = factory.createTreeFromText(xml_text);
+
+    NodeStatus status = NodeStatus::RUNNING;
+    // Keep on ticking until you get either a SUCCESS or FAILURE state
+    while( status == NodeStatus::RUNNING)
+    {
+        status = tree.tickRoot();
+        SleepMS(1);   // optional sleep to avoid "busy loops"
+    }
+
+    // let's visualize some information about the current state of the blackboards.
+    std::cout << "--------------" << std::endl;
+    tree.blackboard_stack[0]->debugMessage();
+    std::cout << "--------------" << std::endl;
+    tree.blackboard_stack[1]->debugMessage();
+    std::cout << "--------------" << std::endl;
+
+    return 0;
+}
+
+/* Expected output:
+
+    [ MoveBase: STARTED ]. goal: x=1 y=2.0 theta=3.00
+    [ MoveBase: FINISHED ]
+    Robot says: mission accomplished
+    --------------
+    move_result (std::string) -> full
+    move_goal (Pose2D) -> full
+    --------------
+    output (std::string) -> remapped to parent [move_result]
+    target (Pose2D) -> remapped to parent [move_goal]
+    --------------
+
+*/
diff --git a/examples/t07_wrap_legacy.cpp b/examples/t07_wrap_legacy.cpp
new file mode 100644 (file)
index 0000000..25ef672
--- /dev/null
@@ -0,0 +1,96 @@
+#include "behaviortree_cpp_v3/bt_factory.h"
+#include "behaviortree_cpp_v3/loggers/bt_cout_logger.h"
+
+/** In this tutorial we will see how to wrap legacy code into a
+ * BehaviorTree in a non-intrusive way, i.e. without modifying the
+ * original class.
+*/
+
+// This is my custom type. We won't know how to read this from a string,
+// unless we implement convertFromString<Point3D>()
+struct Point3D { double x,y,z; };
+
+// We want to create an ActionNode that calls the method MyLegacyMoveTo::go
+class MyLegacyMoveTo
+{
+public:
+    bool go(Point3D goal)
+    {
+        printf("Going to: %f %f %f\n", goal.x, goal.y, goal.z);
+        return true; // true means success in my legacy code
+    }
+};
+
+// Similarly to the previous tutorials, we need to implement this parsing method,
+// providing a specialization of BT::convertFromString
+namespace BT
+{
+template <> Point3D convertFromString(StringView key)
+{
+    // three real numbers separated by semicolons
+    auto parts = BT::splitString(key, ';');
+    if (parts.size() != 3)
+    {
+        throw RuntimeError("invalid input)");
+    }
+    else
+    {
+        Point3D output;
+        output.x  = convertFromString<double>(parts[0]);
+        output.y  = convertFromString<double>(parts[1]);
+        output.z  = convertFromString<double>(parts[2]);
+        return output;
+    }
+}
+} // end anmespace BT
+
+
+// clang-format off
+static const char* xml_text = R"(
+
+ <root>
+     <BehaviorTree>
+        <MoveTo  goal="-1;3;0.5" />
+     </BehaviorTree>
+ </root>
+ )";
+
+// clang-format on
+
+int main()
+{
+    using namespace BT;
+
+    MyLegacyMoveTo move_to;
+
+    // Here we use a lambda that captures the reference of move_to
+    auto MoveToWrapperWithLambda = [&move_to](TreeNode& parent_node) -> NodeStatus
+    {
+        Point3D goal;
+        // thanks to paren_node, you can access easily the input and output ports.
+        parent_node.getInput("goal", goal);
+
+        bool res = move_to.go( goal );
+        // convert bool to NodeStatus
+        return res ? NodeStatus::SUCCESS : NodeStatus::FAILURE;
+    };
+
+    BehaviorTreeFactory factory;
+
+    // Register the lambda with BehaviorTreeFactory::registerSimpleAction
+
+    PortsList ports = { BT::InputPort<Point3D>("goal") };
+    factory.registerSimpleAction("MoveTo", MoveToWrapperWithLambda, ports );
+
+    auto tree = factory.createTreeFromText(xml_text);
+
+    tree.tickRoot();
+
+    return 0;
+}
+
+/* Expected output:
+
+Going to: -1.000000 3.000000 0.500000
+
+*/
diff --git a/examples/t08_additional_node_args.cpp b/examples/t08_additional_node_args.cpp
new file mode 100644 (file)
index 0000000..7ef24f5
--- /dev/null
@@ -0,0 +1,120 @@
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+using namespace BT;
+
+/*
+ * Sometimes, it is convenient to pass additional (static) arguments to a Node.
+ * If these parameters are known at compilation time or at deployment-time
+ * and they don't change at run-time, input ports are probably overkill and cumbersome.
+ *
+ * This tutorial demonstrates two possible ways to initialize a custom node with
+ * additional arguments.
+ */
+
+// Action_A has a different constructor than the default one.
+class Action_A: public SyncActionNode
+{
+
+public:
+    // additional arguments passed to the constructor
+    Action_A(const std::string& name, const NodeConfiguration& config,
+             int arg1, double arg2, std::string arg3 ):
+        SyncActionNode(name, config),
+        _arg1(arg1),
+        _arg2(arg2),
+        _arg3(arg3) {}
+
+    NodeStatus tick() override
+    {
+        std::cout << "Action_A: " << _arg1 << " / " << _arg2 << " / " << _arg3 << std::endl;
+        return NodeStatus::SUCCESS;
+    }
+    static PortsList providedPorts() { return {}; }
+
+private:
+    int _arg1;
+    double _arg2;
+    std::string _arg3;
+};
+
+// Action_B implements an init(...) method that must be called once at the beginning.
+class Action_B: public SyncActionNode
+{
+
+public:
+    Action_B(const std::string& name, const NodeConfiguration& config):
+        SyncActionNode(name, config) {}
+
+    // we want this method to be called ONCE and BEFORE the first tick()
+    void init( int arg1, double arg2, std::string arg3 )
+    {
+        _arg1 = (arg1);
+        _arg2 = (arg2);
+        _arg3 = (arg3);
+    }
+
+    NodeStatus tick() override
+    {
+        std::cout << "Action_B: " << _arg1 << " / " << _arg2 << " / " << _arg3 << std::endl;
+        return NodeStatus::SUCCESS;
+    }
+    static PortsList providedPorts() { return {}; }
+
+private:
+    int _arg1;
+    double _arg2;
+    std::string _arg3;
+};
+
+// Simple tree, used to execute once each action.
+static const char* xml_text = R"(
+
+ <root >
+     <BehaviorTree>
+        <Sequence>
+            <Action_A/>
+            <Action_B/>
+        </Sequence>
+     </BehaviorTree>
+ </root>
+ )";
+
+int main()
+{
+    BehaviorTreeFactory factory;
+
+    // A node builder is nothing more than a function pointer to create a
+    // std::unique_ptr<TreeNode>.
+    // Using lambdas or std::bind, we can easily "inject" additional arguments.
+    NodeBuilder builder_A = [](const std::string& name, const NodeConfiguration& config)
+    {
+        return std::make_unique<Action_A>( name, config, 42, 3.14, "hello world" );
+    };
+
+    // BehaviorTreeFactory::registerBuilder is the more general way to register a custom node.
+    // Not the most user friendly, but definitely the most flexible one.
+    factory.registerBuilder<Action_A>( "Action_A", builder_A);
+
+    // The regitration of  Action_B is done as usual, but we still need to call Action_B::init()
+    factory.registerNodeType<Action_B>( "Action_B" );
+
+    auto tree = factory.createTreeFromText(xml_text);
+
+    // Iterate through all the nodes and call init if it is an Action_B
+    for( auto& node: tree.nodes )
+    {
+        if( auto action_B_node = dynamic_cast<Action_B*>( node.get() ))
+        {
+            action_B_node->init( 69, 9.99, "interesting_value" );
+        }
+    }
+
+    tree.tickRoot();
+
+    /* Expected output:
+
+        Action_A: 42 / 3.14 / hello world
+        Action_B: 69 / 9.99 / interesting_value
+    */
+    return 0;
+}
diff --git a/examples/t09_async_actions_coroutines.cpp b/examples/t09_async_actions_coroutines.cpp
new file mode 100644 (file)
index 0000000..7170e72
--- /dev/null
@@ -0,0 +1,138 @@
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+using namespace BT;
+
+/**
+ * In this tutorial, we demonstrate how to use the CoroActionNode, which
+ * should be preferred over AsyncActionNode when the functions you
+ * use are non-blocking.
+ *
+ */
+
+class MyAsyncAction: public CoroActionNode
+{
+  public:
+    MyAsyncAction(const std::string& name):
+        CoroActionNode(name, {})
+    {}
+
+  private:
+    // This is the ideal skeleton/template of an async action:
+    //  - A request to a remote service provider.
+    //  - A loop where we check if the reply has been received.
+    //  - You may call setStatusRunningAndYield() to "pause".
+    //  - Code to execute after the reply.
+    //  - A simple way to handle halt().
+
+    NodeStatus tick() override
+
+    {
+        std::cout << name() <<": Started. Send Request to server." << std::endl;
+
+        auto Now = [](){ return std::chrono::high_resolution_clock::now(); };
+
+        TimePoint initial_time = Now();
+        TimePoint time_before_reply = initial_time + std::chrono::milliseconds(100);
+
+        int count = 0;
+        bool reply_received = false;
+
+        while( !reply_received )
+        {
+            if( count++ == 0)
+            {
+                // call this only once
+                std::cout << name() <<": Waiting Reply..." << std::endl;
+            }
+            // pretend that we received a reply
+            if( Now() >= time_before_reply )
+            {
+                reply_received = true;
+            }
+
+            if( !reply_received )
+            {
+                // set status to RUNNING and "pause/sleep"
+                // If halt() is called, we will not resume execution (stack destroyed)
+                setStatusRunningAndYield();
+            }
+        }
+
+        // This part of the code is never reached if halt() is invoked,
+        // only if reply_received == true;
+        std::cout << name() <<": Done. 'Waiting Reply' loop repeated "
+                  << count << " times" << std::endl;
+        cleanup(false);
+        return NodeStatus::SUCCESS;
+    }
+
+    // you might want to cleanup differently if it was halted or successful
+    void cleanup(bool halted)
+    {
+        if( halted )
+        {
+            std::cout << name() <<": cleaning up after an halt()\n" << std::endl;
+        }
+        else{
+            std::cout << name() <<": cleaning up after SUCCESS\n" << std::endl;
+        }
+    }
+    void halt() override
+    {
+        std::cout << name() <<": Halted." << std::endl;
+        cleanup(true);
+        // Do not forget to call this at the end.
+        CoroActionNode::halt();
+    }
+};
+
+
+// clang-format off
+static const char* xml_text = R"(
+
+ <root >
+     <BehaviorTree>
+        <Timeout msec="150">
+            <SequenceStar name="sequence">
+                <MyAsyncAction name="action_A"/>
+                <MyAsyncAction name="action_B"/>
+            </SequenceStar>
+        </Timeout>
+     </BehaviorTree>
+ </root>
+ )";
+
+// clang-format on
+
+int main()
+{
+    // Simple tree: a sequence of two asycnhronous actions,
+    // but the second will be halted because of the timeout.
+
+    BehaviorTreeFactory factory;
+    factory.registerNodeType<MyAsyncAction>("MyAsyncAction");
+
+    auto tree = factory.createTreeFromText(xml_text);
+
+    //---------------------------------------
+    // keep executin tick until it returns etiher SUCCESS or FAILURE
+    while( tree.tickRoot() == NodeStatus::RUNNING)
+    {
+        std::this_thread::sleep_for( std::chrono::milliseconds(10) );
+    }
+    return 0;
+}
+
+/* Expected output:
+
+action_A: Started. Send Request to server.
+action_A: Waiting Reply...
+action_A: Done. 'Waiting Reply' loop repeated 11 times
+action_A: cleaning up after SUCCESS
+
+action_B: Started. Send Request to server.
+action_B: Waiting Reply...
+action_B: Halted.
+action_B: cleaning up after an halt()
+
+*/
diff --git a/examples/t10_include_trees.cpp b/examples/t10_include_trees.cpp
new file mode 100644 (file)
index 0000000..2483508
--- /dev/null
@@ -0,0 +1,27 @@
+#include "behaviortree_cpp_v3/bt_factory.h"
+#include "dummy_nodes.h"
+
+using namespace BT;
+
+
+int main(int argc, char** argv)
+{
+    BehaviorTreeFactory factory;
+    DummyNodes::RegisterNodes(factory);
+
+    if( argc != 2)
+    {
+        std::cout <<" missing name of the XML file to open" << std::endl;
+        return 1;
+    }
+
+    // IMPORTANT: when the object tree goes out of scope,
+    // all the TreeNodes are destroyed
+    auto tree = factory.createTreeFromFile(argv[1]);
+
+    printTreeRecursively( tree.rootNode() );
+
+    tree.tickRoot();
+
+    return 0;
+}
diff --git a/examples/t11_runtime_ports.cpp b/examples/t11_runtime_ports.cpp
new file mode 100644 (file)
index 0000000..96a725a
--- /dev/null
@@ -0,0 +1,71 @@
+#include "behaviortree_cpp_v3/bt_factory.h"
+using namespace BT;
+
+// clang-format off
+static const char* xml_text = R"(
+ <root main_tree_to_execute = "MainTree" >
+     <BehaviorTree ID="MainTree">
+        <Sequence name="root">
+            <ThinkRuntimePort   text="{the_answer}"/>
+            <SayRuntimePort     message="{the_answer}" />
+        </Sequence>
+     </BehaviorTree>
+ </root>
+ )";
+// clang-format on
+
+class ThinkRuntimePort: public BT::SyncActionNode
+{
+  public:
+  ThinkRuntimePort(const std::string& name,
+               const BT::NodeConfiguration& config)
+    : BT::SyncActionNode(name, config)
+  {
+  }
+
+  BT::NodeStatus tick() override {
+    setOutput("text", "The answer is 42" );
+    return NodeStatus::SUCCESS;
+  }
+};
+
+class SayRuntimePort : public BT::SyncActionNode
+{
+  public:
+  SayRuntimePort(const std::string& name, const BT::NodeConfiguration& config)
+    : BT::SyncActionNode(name, config)
+  {
+  }
+
+  // You must override the virtual function tick()
+  BT::NodeStatus tick() override
+  {
+    auto msg = getInput<std::string>("message");
+    if (!msg){
+      throw BT::RuntimeError( "missing required input [message]: ", msg.error() );
+    }
+    std::cout << "Robot says: " << msg.value() << std::endl;
+    return BT::NodeStatus::SUCCESS;
+  }
+};
+
+
+int main()
+{
+    BehaviorTreeFactory factory;
+
+    //-------- register ports that might be defined at runtime --------
+    // more verbose way
+    PortsList think_ports = {BT::OutputPort<std::string>("text")};
+    factory.registerBuilder(CreateManifest<ThinkRuntimePort>("ThinkRuntimePort", think_ports),
+                              CreateBuilder<ThinkRuntimePort>());
+    // less verbose way
+    PortsList say_ports = {BT::InputPort<std::string>("message")};
+    factory.registerNodeType<SayRuntimePort>("SayRuntimePort", say_ports);
+
+    auto tree = factory.createTreeFromText(xml_text);
+    tree.tickRoot();
+    return 0;
+}
+
+
diff --git a/examples/t12_ncurses_manual_selector.cpp b/examples/t12_ncurses_manual_selector.cpp
new file mode 100644 (file)
index 0000000..a30647e
--- /dev/null
@@ -0,0 +1,42 @@
+#include "behaviortree_cpp_v3/bt_factory.h"
+#include "dummy_nodes.h"
+
+using namespace BT;
+
+/* Try also
+*      <ManualSelector repeat_last_selection="1">
+*  to see the difference.
+*/
+
+// clang-format off
+static const char* xml_text = R"(
+ <root main_tree_to_execute = "MainTree" >
+     <BehaviorTree ID="MainTree">
+        <Repeat num_cycles="3">
+            <ManualSelector repeat_last_selection="0">
+                <SaySomething name="Option1"    message="Option1" />
+                <SaySomething name="Option2"    message="Option2" />
+                <SaySomething name="Option3"    message="Option3" />
+                <SaySomething name="Option4"    message="Option4" />
+                <ManualSelector name="YouChoose" />
+            </ManualSelector>
+        </Repeat>
+     </BehaviorTree>
+ </root>
+ )";
+// clang-format on
+
+int main()
+{
+    BehaviorTreeFactory factory;
+    factory.registerNodeType<DummyNodes::SaySomething>("SaySomething");
+
+    auto tree = factory.createTreeFromText(xml_text);
+    auto ret = tree.tickRoot();
+
+    std::cout << "Result: " << ret << std::endl;
+
+    return 0;
+}
+
+
diff --git a/examples/test_files/Check.xml b/examples/test_files/Check.xml
new file mode 100644 (file)
index 0000000..50a2e9b
--- /dev/null
@@ -0,0 +1,10 @@
+<root>
+
+    <BehaviorTree ID="CheckStatus">
+        <SequenceStar>
+            <Action ID="CheckBattery"/>
+            <Action ID="CheckTemperature"/>
+        </SequenceStar>
+    </BehaviorTree>
+    
+</root>
diff --git a/examples/test_files/subtree_test.xml b/examples/test_files/subtree_test.xml
new file mode 100644 (file)
index 0000000..b9ccccf
--- /dev/null
@@ -0,0 +1,16 @@
+<root main_tree_to_execute="BehaviorTree">
+       
+    <include path="Check.xml" />
+    <include path="subtrees/Talk.xml" />
+
+    <BehaviorTree ID="BehaviorTree">
+        <Sequence>
+            <CheckStatus/>
+            <OpenGripper/>
+            <CloseGripper/>
+            <SayStuff/>
+        </Sequence>
+    </BehaviorTree>
+
+</root>
+
diff --git a/examples/test_files/subtrees/Talk.xml b/examples/test_files/subtrees/Talk.xml
new file mode 100644 (file)
index 0000000..1d20201
--- /dev/null
@@ -0,0 +1,10 @@
+<root main_tree_to_execute="SayStuff">
+
+    <BehaviorTree ID="SayStuff">
+        <SequenceStar>
+            <Action ID="SaySomething" message="Hello World"/>
+            <Action ID="SayHello"/>
+        </SequenceStar>
+    </BehaviorTree>
+    
+</root>
diff --git a/include/behaviortree_cpp_v3/action_node.h b/include/behaviortree_cpp_v3/action_node.h
new file mode 100644 (file)
index 0000000..a7ef2ae
--- /dev/null
@@ -0,0 +1,225 @@
+/* Copyright (C) 2015-2018 Michele Colledanchise -  All Rights Reserved
+ * Copyright (C) 2018-2020 Davide Faconti, Eurecat -  All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#ifndef BEHAVIORTREECORE_ACTIONNODE_H
+#define BEHAVIORTREECORE_ACTIONNODE_H
+
+#include <atomic>
+#include <thread>
+#include <future>
+#include "leaf_node.h"
+
+namespace BT
+{
+
+// IMPORTANT: Actions which returned SUCCESS or FAILURE will not be ticked
+// again unless setStatus(IDLE) is called first.
+// Keep this in mind when writing your custom Control and Decorator nodes.
+
+
+/**
+ * @brief The ActionNodeBase is the base class to use to create any kind of action.
+ * A particular derived class is free to override executeTick() as needed.
+ *
+ */
+class ActionNodeBase : public LeafNode
+{
+  public:
+
+    ActionNodeBase(const std::string& name, const NodeConfiguration& config);
+    ~ActionNodeBase() override = default;
+
+    virtual NodeType type() const override final
+    {
+        return NodeType::ACTION;
+    }
+};
+
+/**
+ * @brief The SyncActionNode is an ActionNode that
+ * explicitly prevents the status RUNNING and doesn't require
+ * an implementation of halt().
+ */
+class SyncActionNode : public ActionNodeBase
+{
+  public:
+
+    SyncActionNode(const std::string& name, const NodeConfiguration& config);
+    ~SyncActionNode() override = default;
+
+    /// throws if the derived class return RUNNING.
+    virtual NodeStatus executeTick() override;
+
+    /// You don't need to override this
+    virtual void halt() override final
+    {
+        setStatus(NodeStatus::IDLE);
+    }
+};
+
+/**
+ * @brief The SimpleActionNode provides an easy to use SyncActionNode.
+ * The user should simply provide a callback with this signature
+ *
+ *    BT::NodeStatus functionName(TreeNode&)
+ *
+ * This avoids the hassle of inheriting from a ActionNode.
+ *
+ * Using lambdas or std::bind it is easy to pass a pointer to a method.
+ * SimpleActionNode is executed synchronously and does not support halting.
+ * NodeParameters aren't supported.
+ */
+class SimpleActionNode : public SyncActionNode
+{
+  public:
+    typedef std::function<NodeStatus(TreeNode&)> TickFunctor;
+
+    // You must provide the function to call when tick() is invoked
+    SimpleActionNode(const std::string& name, TickFunctor tick_functor,
+                     const NodeConfiguration& config);
+
+    ~SimpleActionNode() override = default;
+
+  protected:
+    virtual NodeStatus tick() override final;
+
+    TickFunctor tick_functor_;
+};
+
+/**
+ * @brief The AsyncActionNode uses a different thread, where the action will be
+ * executed.
+ *
+ * IMPORTANT: this action is quite hard to implement correctly. Please be sure that you know what you are doing.
+ *
+ * - In your overriden tick() method, you must check periodically
+ *   the result of the method isHaltRequested() and stop your execution accordingly.
+ *
+ * - in the overriden halt() method, you can do some cleanup, but do not forget to
+ *   invoke the base class method AsyncActionNode::halt();
+ *
+ * - remember, with few exceptions, a halted AsyncAction must return NodeStatus::IDLE.
+ *
+ * For a complete example, look at __AsyncActionTest__ in action_test_node.h in the folder test.
+ */
+class AsyncActionNode : public ActionNodeBase
+{
+  public:
+
+    AsyncActionNode(const std::string& name, const NodeConfiguration& config):ActionNodeBase(name, config)
+    {
+    }
+
+    bool isHaltRequested() const
+    {
+        return halt_requested_.load();
+    }
+
+    // This method spawn a new thread. Do NOT remove the "final" keyword.
+    virtual NodeStatus executeTick() override final;
+
+    virtual void halt() override;
+
+  private:
+
+    std::exception_ptr exptr_;
+    std::atomic_bool halt_requested_;
+    std::future<NodeStatus> thread_handle_;
+};
+
+/**
+ * @brief The ActionNode is the goto option for,
+ * but it is actually much easier to use correctly.
+ *
+ * It is particularly useful when your code contains a request-reply pattern,
+ * i.e. when the actions sends an asychronous request, then checks periodically
+ * if the reply has been received and, eventually, analyze the reply to determine
+ * if the result is SUCCESS or FAILURE.
+ *
+ * -) an action that was in IDLE state will call onStart()
+ *
+ * -) A RUNNING action will call onRunning()
+ *
+ * -) if halted, method onHalted() is invoked
+ */
+class StatefulActionNode : public ActionNodeBase
+{
+  public:
+      StatefulActionNode(const std::string& name, const NodeConfiguration& config):
+        ActionNodeBase(name,config)
+      {}
+
+      // do not override this method
+      NodeStatus tick() override final;
+      // do not override this method
+      void halt() override final;
+
+      /// method to be called at the beginning.
+      /// If it returns RUNNING, this becomes an asychronous node.
+      virtual NodeStatus onStart() = 0;
+
+      /// method invoked by a RUNNING action.
+      virtual NodeStatus onRunning() = 0;
+
+      /// when the method halt() is called and the action is RUNNING, this method is invoked.
+      /// This is a convenient place todo a cleanup, if needed.
+      virtual void onHalted() = 0;
+};
+
+
+#ifndef BT_NO_COROUTINES
+
+/**
+ * @brief The CoroActionNode class is an ideal candidate for asynchronous actions
+ * which need to communicate with an external service using an asynch request/reply interface
+ * (being notable examples ActionLib in ROS, MoveIt clients or move_base clients).
+ *
+ * It is up to the user to decide when to suspend execution of the Action and resume
+ * the parent node, invoking the method setStatusRunningAndYield().
+ */
+class CoroActionNode : public ActionNodeBase
+{
+  public:
+
+    CoroActionNode(const std::string& name, const NodeConfiguration& config);
+    virtual ~CoroActionNode() override;
+
+    /// Use this method to return RUNNING and temporary "pause" the Action.
+    void setStatusRunningAndYield();
+
+    // This method triggers the TickEngine. Do NOT remove the "final" keyword.
+    virtual NodeStatus executeTick() override final;
+
+    /** You may want to override this method. But still, remember to call this
+    * implementation too.
+    *
+    * Example:
+    *
+    *     void MyAction::halt()
+    *     {
+    *         // do your stuff here
+    *         CoroActionNode::halt();
+    *     }
+    */
+    void halt() override;
+
+  protected:
+
+    struct Pimpl; // The Pimpl idiom
+    std::unique_ptr<Pimpl> _p;
+};
+#endif
+
+}   //end namespace
+
+#endif
diff --git a/include/behaviortree_cpp_v3/actions/always_failure_node.h b/include/behaviortree_cpp_v3/actions/always_failure_node.h
new file mode 100644 (file)
index 0000000..495cb44
--- /dev/null
@@ -0,0 +1,40 @@
+/*  Copyright (C) 2018-2020 Davide Faconti, Eurecat -  All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#ifndef ACTION_ALWAYS_FAILURE_NODE_H
+#define ACTION_ALWAYS_FAILURE_NODE_H
+
+#include "behaviortree_cpp_v3/action_node.h"
+
+namespace BT
+{
+/**
+ * Simple actions that always returns FAILURE.
+ */
+class AlwaysFailureNode : public SyncActionNode
+{
+  public:
+    AlwaysFailureNode(const std::string& name) :
+        SyncActionNode(name, {})
+    {
+        setRegistrationID("AlwaysFailure");
+    }
+
+  private:
+    virtual BT::NodeStatus tick() override
+    {
+        return NodeStatus::FAILURE;
+    }
+};
+}
+
+#endif
diff --git a/include/behaviortree_cpp_v3/actions/always_success_node.h b/include/behaviortree_cpp_v3/actions/always_success_node.h
new file mode 100644 (file)
index 0000000..5af2198
--- /dev/null
@@ -0,0 +1,40 @@
+/*  Copyright (C) 2018-2020 Davide Faconti, Eurecat -  All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#ifndef ACTION_ALWAYS_SUCCESS_NODE_H
+#define ACTION_ALWAYS_SUCCESS_NODE_H
+
+#include "behaviortree_cpp_v3/action_node.h"
+
+namespace BT
+{
+/**
+ * Simple actions that always returns SUCCESS.
+ */
+class AlwaysSuccessNode : public SyncActionNode
+{
+  public:
+    AlwaysSuccessNode(const std::string& name) :
+        SyncActionNode(name, {} )
+    {
+        setRegistrationID("AlwaysSuccess");
+    }
+
+  private:
+    virtual BT::NodeStatus tick() override
+    {
+        return NodeStatus::SUCCESS;
+    }
+};
+}
+
+#endif
diff --git a/include/behaviortree_cpp_v3/actions/set_blackboard_node.h b/include/behaviortree_cpp_v3/actions/set_blackboard_node.h
new file mode 100644 (file)
index 0000000..eaaeab8
--- /dev/null
@@ -0,0 +1,63 @@
+/*  Copyright (C) 2018-2020 Davide Faconti, Eurecat -  All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#ifndef ACTION_SETBLACKBOARD_NODE_H
+#define ACTION_SETBLACKBOARD_NODE_H
+
+#include "behaviortree_cpp_v3/action_node.h"
+
+namespace BT
+{
+/**
+ * @brief The SetBlackboard is action used to store a string
+ * into an entry of the Blackboard specified in "output_key".
+ *
+ * Example usage:
+ *
+ *  <SetBlackboard value="42" output_key="the_answer" />
+ *
+ * Will store the string "42" in the entry with key "the_answer".
+ */
+class SetBlackboard : public SyncActionNode
+{
+  public:
+    SetBlackboard(const std::string& name, const NodeConfiguration& config)
+      : SyncActionNode(name, config)
+    {
+        setRegistrationID("SetBlackboard");
+    }
+
+    static PortsList providedPorts()
+    {
+        return { InputPort("value", "Value represented as a string. convertFromString must be implemented."),
+                 BidirectionalPort("output_key", "Name of the blackboard entry where the value should be written") };
+    }
+
+  private:
+    virtual BT::NodeStatus tick() override
+    {
+        std::string key, value;
+        if ( !getInput("output_key", key) )
+        {
+            throw RuntimeError("missing port [output_key]");
+        }
+        if ( !getInput("value", value) )
+        {
+            throw RuntimeError("missing port [value]");
+        }
+        setOutput("output_key", value);
+        return NodeStatus::SUCCESS;
+    }
+};
+}
+
+#endif
diff --git a/include/behaviortree_cpp_v3/basic_types.h b/include/behaviortree_cpp_v3/basic_types.h
new file mode 100644 (file)
index 0000000..0f301bd
--- /dev/null
@@ -0,0 +1,345 @@
+ #ifndef BT_BASIC_TYPES_H
+#define BT_BASIC_TYPES_H
+
+#include <iostream>
+#include <vector>
+#include <sstream>
+#include <unordered_map>
+#include <unordered_set>
+#include <typeinfo>
+#include <functional>
+#include <chrono>
+#include <memory>
+#include "behaviortree_cpp_v3/utils/string_view.hpp"
+#include "behaviortree_cpp_v3/utils/safe_any.hpp"
+#include "behaviortree_cpp_v3/exceptions.h"
+#include "behaviortree_cpp_v3/utils/expected.hpp"
+#include "behaviortree_cpp_v3/utils/make_unique.hpp"
+
+namespace BT
+{
+/// Enumerates the possible types of nodes
+enum class NodeType
+{
+    UNDEFINED = 0,
+    ACTION,
+    CONDITION,
+    CONTROL,
+    DECORATOR,
+    SUBTREE
+};
+
+/// Enumerates the states every node can be in after execution during a particular
+/// time step.
+/// IMPORTANT: Your custom nodes should NEVER return IDLE.
+enum class NodeStatus
+{
+    IDLE = 0,
+    RUNNING,
+    SUCCESS,
+    FAILURE
+};
+
+enum class PortDirection{
+    INPUT,
+    OUTPUT,
+    INOUT
+};
+
+
+typedef nonstd::string_view StringView;
+
+/**
+ * convertFromString is used to convert a string into a custom type.
+ *
+ * This function is invoked under the hood by TreeNode::getInput(), but only when the
+ * input port contains a string.
+ *
+ * If you have a custom type, you need to implement the corresponding template specialization.
+ */
+template <typename T> inline
+T convertFromString(StringView /*str*/)
+{
+    auto type_name = BT::demangle( typeid(T) );
+
+    std::cerr << "You (maybe indirectly) called BT::convertFromString() for type [" <<
+                 type_name <<"], but I can't find the template specialization.\n" << std::endl;
+
+    throw LogicError(std::string("You didn't implement the template specialization of "
+                                 "convertFromString for this type: ") + type_name );
+}
+
+template <>
+std::string convertFromString<std::string>(StringView str);
+
+template <>
+const char* convertFromString<const char*>(StringView str);
+
+template <>
+int convertFromString<int>(StringView str);
+
+template <>
+unsigned convertFromString<unsigned>(StringView str);
+
+template <>
+long convertFromString<long>(StringView str);
+
+template <>
+unsigned long convertFromString<unsigned long>(StringView str);
+
+template <>
+float convertFromString<float>(StringView str);
+
+template <>
+double convertFromString<double>(StringView str);
+
+template <> // Integer numbers separated by the characted ";"
+std::vector<int> convertFromString<std::vector<int>>(StringView str);
+
+template <> // Real numbers separated by the characted ";"
+std::vector<double> convertFromString<std::vector<double>>(StringView str);
+
+template <> // This recognizes either 0/1, true/false, TRUE/FALSE
+bool convertFromString<bool>(StringView str);
+
+template <> // Names with all capital letters
+NodeStatus convertFromString<NodeStatus>(StringView str);
+
+template <>  // Names with all capital letters
+NodeType convertFromString<NodeType>(StringView str);
+
+template <>
+PortDirection convertFromString<PortDirection>(StringView str);
+
+
+typedef std::function<Any(StringView)> StringConverter;
+
+typedef std::unordered_map<const std::type_info*, StringConverter> StringConvertersMap;
+
+
+// helper function
+template <typename T> inline
+StringConverter GetAnyFromStringFunctor()
+{
+    return [](StringView str){ return Any(convertFromString<T>(str)); };
+}
+
+template <> inline
+StringConverter GetAnyFromStringFunctor<void>()
+{
+    return {};
+}
+
+//------------------------------------------------------------------
+
+template <typename T>
+std::string toStr(T value)
+{
+    return std::to_string(value);
+}
+
+std::string toStr(std::string value);
+
+template<> std::string toStr<BT::NodeStatus>(BT::NodeStatus status);
+
+/**
+ * @brief toStr converts NodeStatus to string. Optionally colored.
+ */
+std::string toStr(BT::NodeStatus status, bool colored);
+
+std::ostream& operator<<(std::ostream& os, const BT::NodeStatus& status);
+
+/**
+ * @brief toStr converts NodeType to string.
+ */
+template<> std::string toStr<BT::NodeType>(BT::NodeType type);
+
+std::ostream& operator<<(std::ostream& os, const BT::NodeType& type);
+
+
+template<> std::string toStr<BT::PortDirection>(BT::PortDirection direction);
+
+std::ostream& operator<<(std::ostream& os, const BT::PortDirection& type);
+
+// Small utility, unless you want to use <boost/algorithm/string.hpp>
+std::vector<StringView> splitString(const StringView& strToSplit, char delimeter);
+
+template <typename Predicate>
+using enable_if = typename std::enable_if< Predicate::value >::type*;
+
+template <typename Predicate>
+using enable_if_not = typename std::enable_if< !Predicate::value >::type*;
+
+
+/** Usage: given a function/method like:
+ *
+ *     Optional<double> getAnswer();
+ *
+ * User code can check result and error message like this:
+ *
+ *     auto res = getAnswer();
+ *     if( res )
+ *     {
+ *         std::cout << "answer was: " << res.value() << std::endl;
+ *     }
+ *     else{
+ *         std::cerr << "failed to get the answer: " << res.error() << std::endl;
+ *     }
+ *
+ * */
+template <typename T> using Optional = nonstd::expected<T, std::string>;
+// note: we use the name Optional instead of expected because it is more intuitive
+// for users that are not up to date with "modern" C++
+
+/** Usage: given a function/method like:
+ *
+ *     Result DoSomething();
+ *
+ * User code can check result and error message like this:
+ *
+ *     auto res = DoSomething();
+ *     if( res )
+ *     {
+ *         std::cout << "DoSomething() done " << std::endl;
+ *     }
+ *     else{
+ *         std::cerr << "DoSomething() failed with message: " << res.error() << std::endl;
+ *     }
+ *
+ * */
+using Result = Optional<void>;
+
+class PortInfo
+{
+
+public:
+  PortInfo( PortDirection direction = PortDirection::INOUT  ):
+        _type(direction), _info(nullptr) {}
+
+    PortInfo( PortDirection direction,
+              const std::type_info& type_info,
+              StringConverter conv):
+        _type(direction),
+        _info( &type_info ),
+        _converter(conv)
+    {}
+
+    PortDirection direction() const;
+
+    const std::type_info* type() const;
+
+    Any parseString(const char *str) const;
+
+    Any parseString(const std::string& str) const;
+
+    template <typename T>
+    Any parseString(const T& ) const
+    {
+        // avoid compilation errors
+        return {};
+    }
+
+    void setDescription(StringView description);
+
+    void setDefaultValue(StringView default_value_as_string);
+
+    const std::string& description() const;
+
+    const std::string& defaultValue() const;
+
+private:
+
+    PortDirection _type;
+    const std::type_info* _info;
+    StringConverter _converter;
+    std::string description_;
+    std::string default_value_;
+};
+
+template <typename T = void>
+std::pair<std::string,PortInfo> CreatePort(PortDirection direction,
+                                           StringView name,
+                                           StringView description = {})
+{
+    std::pair<std::string,PortInfo> out;
+
+    if( std::is_same<T, void>::value)
+    {
+        out = {static_cast<std::string>(name), PortInfo(direction) };
+    }
+    else{
+        out =  {static_cast<std::string>(name), PortInfo(direction, typeid(T),
+                                          GetAnyFromStringFunctor<T>() ) };
+    }
+    if( !description.empty() )
+    {
+        out.second.setDescription(description);
+    }
+    return out;
+}
+
+//----------
+template <typename T = void> inline
+    std::pair<std::string,PortInfo> InputPort(StringView name, StringView description = {})
+{
+    return CreatePort<T>(PortDirection::INPUT, name, description );
+}
+
+template <typename T = void> inline
+std::pair<std::string,PortInfo> OutputPort(StringView name, StringView description = {})
+{
+    return CreatePort<T>(PortDirection::OUTPUT, name, description );
+}
+
+template <typename T = void> inline
+    std::pair<std::string,PortInfo> BidirectionalPort(StringView name, StringView description = {})
+{
+    return CreatePort<T>(PortDirection::INOUT, name, description );
+}
+//----------
+template <typename T = void> inline
+    std::pair<std::string,PortInfo> InputPort(StringView name, const T& default_value, StringView description)
+{
+    auto out = CreatePort<T>(PortDirection::INPUT, name, description );
+    out.second.setDefaultValue( BT::toStr(default_value) );
+    return out;
+}
+
+template <typename T = void> inline
+    std::pair<std::string,PortInfo> BidirectionalPort(StringView name, const T& default_value, StringView description)
+{
+    auto out = CreatePort<T>(PortDirection::INOUT, name, description );
+    out.second.setDefaultValue( BT::toStr(default_value) );
+    return out;
+}
+//----------
+
+typedef std::unordered_map<std::string, PortInfo> PortsList;
+
+template <typename T, typename = void>
+struct has_static_method_providedPorts: std::false_type {};
+
+template <typename T>
+struct has_static_method_providedPorts<T,
+        typename std::enable_if<std::is_same<decltype(T::providedPorts()), PortsList>::value>::type>
+    : std::true_type {};
+
+template <typename T> inline
+PortsList getProvidedPorts(enable_if< has_static_method_providedPorts<T> > = nullptr)
+{
+    return T::providedPorts();
+}
+
+template <typename T> inline
+PortsList getProvidedPorts(enable_if_not< has_static_method_providedPorts<T> > = nullptr)
+{
+    return {};
+}
+
+typedef std::chrono::high_resolution_clock::time_point TimePoint;
+typedef std::chrono::high_resolution_clock::duration Duration;
+
+} // end namespace
+
+
+#endif   // BT_BASIC_TYPES_H
diff --git a/include/behaviortree_cpp_v3/behavior_tree.h b/include/behaviortree_cpp_v3/behavior_tree.h
new file mode 100644 (file)
index 0000000..0e70214
--- /dev/null
@@ -0,0 +1,93 @@
+/* Copyright (C) 2015-2018 Michele Colledanchise -  All Rights Reserved
+ * Copyright (C) 2018-2020 Davide Faconti, Eurecat -  All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#ifndef BEHAVIOR_TREE_H
+#define BEHAVIOR_TREE_H
+
+#include "behaviortree_cpp_v3/controls/parallel_node.h"
+#include "behaviortree_cpp_v3/controls/reactive_sequence.h"
+#include "behaviortree_cpp_v3/controls/reactive_fallback.h"
+#include "behaviortree_cpp_v3/controls/fallback_node.h"
+#include "behaviortree_cpp_v3/controls/sequence_node.h"
+#include "behaviortree_cpp_v3/controls/sequence_star_node.h"
+#include "behaviortree_cpp_v3/controls/switch_node.h"
+#include "behaviortree_cpp_v3/controls/manual_node.h"
+#include "behaviortree_cpp_v3/controls/if_then_else_node.h"
+#include "behaviortree_cpp_v3/controls/while_do_else_node.h"
+
+#include "behaviortree_cpp_v3/action_node.h"
+#include "behaviortree_cpp_v3/condition_node.h"
+
+#include "behaviortree_cpp_v3/decorators/inverter_node.h"
+#include "behaviortree_cpp_v3/decorators/retry_node.h"
+#include "behaviortree_cpp_v3/decorators/repeat_node.h"
+#include "behaviortree_cpp_v3/decorators/subtree_node.h"
+
+#include "behaviortree_cpp_v3/actions/always_success_node.h"
+#include "behaviortree_cpp_v3/actions/always_failure_node.h"
+#include "behaviortree_cpp_v3/actions/set_blackboard_node.h"
+
+#include "behaviortree_cpp_v3/decorators/force_success_node.h"
+#include "behaviortree_cpp_v3/decorators/force_failure_node.h"
+#include "behaviortree_cpp_v3/decorators/keep_running_until_failure_node.h"
+#include "behaviortree_cpp_v3/decorators/blackboard_precondition.h"
+#include "behaviortree_cpp_v3/decorators/timeout_node.h"
+#include "behaviortree_cpp_v3/decorators/delay_node.h"
+
+
+namespace BT
+{
+
+//Call the visitor for each node of the tree, given a root.
+void applyRecursiveVisitor(const TreeNode* root_node,
+                           const std::function<void(const TreeNode*)>& visitor);
+
+//Call the visitor for each node of the tree, given a root.
+void applyRecursiveVisitor(TreeNode* root_node, const std::function<void(TreeNode*)>& visitor);
+
+/**
+ * Debug function to print on screen the hierarchy of the tree.
+ */
+void printTreeRecursively(const TreeNode* root_node);
+
+typedef std::vector<std::pair<uint16_t, uint8_t>> SerializedTreeStatus;
+
+/**
+ * @brief buildSerializedStatusSnapshot can be used to create a buffer that can be stored
+ * (or sent to a client application) to know the status of all the nodes of a tree.
+ * It is not "human readable".
+ *
+ * @param root_node
+ * @param serialized_buffer is the output.
+ */
+void buildSerializedStatusSnapshot(const TreeNode* root_node,
+                                   SerializedTreeStatus& serialized_buffer);
+
+/// Simple way to extract the type of a TreeNode at COMPILE TIME.
+/// Useful to avoid the cost of dynamic_cast or the virtual method TreeNode::type().
+template <typename T>
+inline NodeType getType()
+{
+    // clang-format off
+    if( std::is_base_of<ActionNodeBase, T>::value )        return NodeType::ACTION;
+    if( std::is_base_of<ConditionNode, T>::value )         return NodeType::CONDITION;
+    if( std::is_base_of<SubtreeNode, T>::value )           return NodeType::SUBTREE;
+    if( std::is_base_of<SubtreePlusNode, T>::value )       return NodeType::SUBTREE;
+    if( std::is_base_of<DecoratorNode, T>::value )         return NodeType::DECORATOR;
+    if( std::is_base_of<ControlNode, T>::value )           return NodeType::CONTROL;
+    return NodeType::UNDEFINED;
+    // clang-format on
+}
+}
+
+#endif   // BEHAVIOR_TREE_H
diff --git a/include/behaviortree_cpp_v3/blackboard.h b/include/behaviortree_cpp_v3/blackboard.h
new file mode 100644 (file)
index 0000000..c248f49
--- /dev/null
@@ -0,0 +1,218 @@
+#ifndef BLACKBOARD_H
+#define BLACKBOARD_H
+
+#include <iostream>
+#include <string>
+#include <memory>
+#include <stdint.h>
+#include <unordered_map>
+#include <mutex>
+#include <sstream>
+
+#include "behaviortree_cpp_v3/basic_types.h"
+#include "behaviortree_cpp_v3/utils/safe_any.hpp"
+#include "behaviortree_cpp_v3/exceptions.h"
+
+namespace BT
+{
+
+/**
+ * @brief The Blackboard is the mechanism used by BehaviorTrees to exchange
+ * typed data.
+ */
+class Blackboard
+{
+  public:
+    typedef std::shared_ptr<Blackboard> Ptr;
+
+  protected:
+    // This is intentionally protected. Use Blackboard::create instead
+    Blackboard(Blackboard::Ptr parent): parent_bb_(parent)
+    {}
+
+  public:
+
+    /** Use this static method to create an instance of the BlackBoard
+    *   to share among all your NodeTrees.
+    */
+    static Blackboard::Ptr create(Blackboard::Ptr parent = {})
+    {
+        return std::shared_ptr<Blackboard>( new Blackboard(parent) );
+    }
+
+    virtual ~Blackboard() = default;
+
+    /**
+     * @brief The method getAny allow the user to access directly the type
+     * erased value.
+     *
+     * @return the pointer or nullptr if it fails.
+     */
+    const Any* getAny(const std::string& key) const
+    {
+        std::unique_lock<std::mutex> lock(mutex_);
+
+        if( auto parent = parent_bb_.lock())
+        {
+            auto remapping_it = internal_to_external_.find(key);
+            if( remapping_it != internal_to_external_.end())
+            {
+                return parent->getAny( remapping_it->second );
+            }
+        }
+        auto it = storage_.find(key);
+        return ( it == storage_.end()) ? nullptr : &(it->second.value);
+    }
+
+    Any* getAny(const std::string& key)
+    {
+        std::unique_lock<std::mutex> lock(mutex_);
+
+        if( auto parent = parent_bb_.lock())
+        {
+            auto remapping_it = internal_to_external_.find(key);
+            if( remapping_it != internal_to_external_.end())
+            {
+                return parent->getAny( remapping_it->second );
+            }
+        }
+        auto it = storage_.find(key);
+        return ( it == storage_.end()) ? nullptr : &(it->second.value);
+    }
+
+    /** Return true if the entry with the given key was found.
+     *  Note that this method may throw an exception if the cast to T failed.
+     */
+    template <typename T>
+    bool get(const std::string& key, T& value) const
+    {
+        const Any* val = getAny(key);
+        if (val)
+        {
+            value = val->cast<T>();
+        }
+        return (bool)val;
+    }
+
+    /**
+     * Version of get() that throws if it fails.
+    */
+    template <typename T>
+    T get(const std::string& key) const
+    {
+        const Any* val = getAny(key);
+        if (val)
+        {
+            return val->cast<T>();
+        }
+        else
+        {
+            throw RuntimeError("Blackboard::get() error. Missing key [", key, "]");
+        }
+    }
+
+
+    /// Update the entry with the given key
+    template <typename T>
+    void set(const std::string& key, const T& value)
+    {
+        std::unique_lock<std::mutex> lock(mutex_);
+        auto it = storage_.find(key);
+
+        if( auto parent = parent_bb_.lock())
+        {
+            auto remapping_it = internal_to_external_.find(key);
+            if( remapping_it != internal_to_external_.end())
+            {
+                const auto& remapped_key = remapping_it->second;
+                if( it == storage_.end() ) // virgin entry
+                {
+                    auto parent_info = parent->portInfo(remapped_key);
+                    if( parent_info )
+                    {
+                        storage_.insert( {key, Entry( *parent_info ) } );
+                    }
+                    else{
+                        storage_.insert( {key, Entry( PortInfo() ) } );
+                    }
+                }
+                parent->set( remapped_key, value );
+                return;
+            }
+        }
+
+        if( it != storage_.end() ) // already there. check the type
+        {
+            const PortInfo& port_info = it->second.port_info;
+            auto& previous_any = it->second.value;
+            const auto locked_type = port_info.type();
+
+            Any temp(value);
+
+            if( locked_type && *locked_type != typeid(T) && *locked_type != temp.type() )
+            {
+                bool mismatching = true;
+                if( std::is_constructible<StringView, T>::value )
+                {
+                    Any any_from_string = port_info.parseString( value );
+                    if( any_from_string.empty() == false)
+                    {
+                        mismatching = false;
+                        temp = std::move( any_from_string );
+                    }
+                }
+
+                if( mismatching )
+                {
+                    debugMessage();
+
+                    throw LogicError( "Blackboard::set() failed: once declared, the type of a port shall not change. "
+                                     "Declared type [", demangle( locked_type ),
+                                     "] != current type [", demangle( typeid(T) ),"]" );
+                }
+            }
+            previous_any = std::move(temp);
+        }
+        else{ // create for the first time without any info
+            storage_.emplace( key, Entry( Any(value), PortInfo() ) );
+        }
+        return;
+    }
+
+    void setPortInfo(std::string key, const PortInfo& info);
+
+    const PortInfo *portInfo(const std::string& key);
+
+    void addSubtreeRemapping(StringView internal, StringView external);
+
+    void debugMessage() const;
+
+    std::vector<StringView> getKeys() const;
+
+  private:
+
+    struct Entry{
+        Any value;
+        const PortInfo port_info;
+
+        Entry( const PortInfo& info ):
+          port_info(info)
+        {}
+
+        Entry(Any&& other_any, const PortInfo& info):
+          value(std::move(other_any)),
+          port_info(info)
+        {}
+    };
+
+    mutable std::mutex mutex_;
+    std::unordered_map<std::string, Entry> storage_;
+    std::weak_ptr<Blackboard> parent_bb_;
+    std::unordered_map<std::string,std::string> internal_to_external_;
+
+};
+
+
+} // end namespace
+
+#endif   // BLACKBOARD_H
diff --git a/include/behaviortree_cpp_v3/bt_factory.h b/include/behaviortree_cpp_v3/bt_factory.h
new file mode 100644 (file)
index 0000000..ac7c424
--- /dev/null
@@ -0,0 +1,388 @@
+/* Copyright (C) 2018 Michele Colledanchise -  All Rights Reserved
+ * Copyright (C) 2018-2020 Davide Faconti, Eurecat -  All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#ifndef BT_FACTORY_H
+#define BT_FACTORY_H
+
+#include <functional>
+#include <memory>
+#include <unordered_map>
+#include <unordered_set>
+#include <cstring>
+#include <algorithm>
+#include <set>
+
+
+#include "behaviortree_cpp_v3/behavior_tree.h"
+
+namespace BT
+{
+
+/// The term "Builder" refers to the Builder Pattern (https://en.wikipedia.org/wiki/Builder_pattern)
+typedef std::function<std::unique_ptr<TreeNode>(const std::string&, const NodeConfiguration&)>
+NodeBuilder;
+
+template <typename T>
+using has_default_constructor = typename std::is_constructible<T, const std::string&>;
+
+template <typename T>
+using has_params_constructor  = typename std::is_constructible<T, const std::string&, const NodeConfiguration&>;
+
+
+template <typename T> inline
+  NodeBuilder CreateBuilder(typename std::enable_if<has_default_constructor<T>::value &&
+                                        has_params_constructor<T>::value >::type* = nullptr)
+{
+  return [](const std::string& name, const NodeConfiguration& config)
+  {
+    // Special case. Use default constructor if parameters are empty
+    if( config.input_ports.empty() &&
+        config.output_ports.empty() &&
+        has_default_constructor<T>::value)
+    {
+      return std::make_unique<T>(name);
+    }
+    return std::make_unique<T>(name, config);
+  };
+}
+
+template <typename T> inline
+  NodeBuilder CreateBuilder(typename std::enable_if<!has_default_constructor<T>::value &&
+                                        has_params_constructor<T>::value >::type* = nullptr)
+{
+  return [](const std::string& name, const NodeConfiguration& params)
+  {
+    return std::unique_ptr<TreeNode>(new T(name, params));
+  };
+}
+
+template <typename T> inline
+  NodeBuilder CreateBuilder(typename std::enable_if<has_default_constructor<T>::value &&
+                                        !has_params_constructor<T>::value >::type* = nullptr)
+{
+  return [](const std::string& name, const NodeConfiguration&)
+  {
+    return std::unique_ptr<TreeNode>(new T(name));
+  };
+}
+
+
+template <typename T> inline
+TreeNodeManifest CreateManifest(const std::string& ID, PortsList portlist = getProvidedPorts<T>())
+{
+  return { getType<T>(), ID, portlist };
+}
+
+
+constexpr const char* PLUGIN_SYMBOL = "BT_RegisterNodesFromPlugin";
+
+#ifndef BT_PLUGIN_EXPORT
+
+/* Use this macro to automatically register one or more custom Nodes
+into a factory. For instance:
+
+BT_REGISTER_NODES(factory)
+{
+    factory.registerNodeType<MoveBaseAction>("MoveBase");
+}
+
+IMPORTANT: this must funtion MUST be declared in a cpp file, NOT a header file.
+See examples for more information about configuring CMake correctly
+*/
+#define BT_REGISTER_NODES(factory)                                                                 \
+        static void BT_RegisterNodesFromPlugin(BT::BehaviorTreeFactory& factory)
+
+#else
+
+#if defined(__linux__) || defined __APPLE__
+
+#define BT_REGISTER_NODES(factory)                                                                 \
+    extern "C" void __attribute__((visibility("default")))                                         \
+        BT_RegisterNodesFromPlugin(BT::BehaviorTreeFactory& factory)
+
+#elif _WIN32
+
+#define BT_REGISTER_NODES(factory)                                                                 \
+    extern "C" void __declspec(dllexport) BT_RegisterNodesFromPlugin(BT::BehaviorTreeFactory& factory)
+#endif
+
+#endif
+
+
+/**
+ * @brief Struct used to store a tree.
+ * If this object goes out of scope, the tree is destroyed.
+ *
+ * To tick the tree, simply call:
+ *
+ *    NodeStatus status = my_tree.tickRoot();
+ */
+class Tree
+{
+public:
+
+    std::vector<TreeNode::Ptr> nodes;
+    std::vector<Blackboard::Ptr> blackboard_stack;
+    std::unordered_map<std::string, TreeNodeManifest> manifests;
+
+    Tree(){}
+
+    // non-copyable. Only movable
+    Tree(const Tree& ) = delete;
+    Tree& operator=(const Tree&) = delete;
+
+    Tree(Tree&& other)
+    {
+        (*this) = std::move(other);
+    }
+
+    Tree& operator=(Tree&& other)
+    {
+        nodes = std::move(other.nodes);
+        blackboard_stack = std::move(other.blackboard_stack);
+        manifests = std::move(other.manifests);
+        return *this;
+    }
+
+    void haltTree()
+    {
+        if(!rootNode())
+        {
+            return;
+        }
+        // the halt should propagate to all the node if the nodes
+        // have been implemented correctly
+        rootNode()->halt();
+        rootNode()->setStatus(NodeStatus::IDLE);
+
+        //but, just in case.... this should be no-op
+        auto visitor = [](BT::TreeNode * node) {
+            node->halt();
+            node->setStatus(BT::NodeStatus::IDLE);
+        };
+        BT::applyRecursiveVisitor(rootNode(), visitor);
+    }
+
+    TreeNode* rootNode() const
+    {
+      return nodes.empty() ? nullptr : nodes.front().get();
+    }
+
+    NodeStatus tickRoot()
+    {
+      if(!rootNode())
+      {
+        throw RuntimeError("Empty Tree");
+      }
+      NodeStatus ret = rootNode()->executeTick();
+      if( ret == NodeStatus::SUCCESS || ret == NodeStatus::FAILURE){
+        rootNode()->setStatus(BT::NodeStatus::IDLE);
+      }
+      return ret;
+    }
+
+    ~Tree();
+
+    Blackboard::Ptr rootBlackboard();
+
+};
+
+/**
+ * @brief The BehaviorTreeFactory is used to create instances of a
+ * TreeNode at run-time.
+ *
+ * Some node types are "builtin", whilst other are used defined and need
+ * to be registered using a unique ID.
+ */
+class BehaviorTreeFactory
+{
+public:
+    BehaviorTreeFactory();
+
+    /// Remove a registered ID.
+    bool unregisterBuilder(const std::string& ID);
+
+    /// The most generic way to register your own builder.
+    void registerBuilder(const TreeNodeManifest& manifest, const NodeBuilder& builder);
+
+    template <typename T>
+    void registerBuilder(const std::string& ID, const NodeBuilder& builder )
+    {
+        auto manifest = CreateManifest<T>(ID);
+        registerBuilder(manifest, builder);
+    }
+
+    /**
+    * @brief registerSimpleAction help you register nodes of type SimpleActionNode.
+    *
+    * @param ID            registration ID
+    * @param tick_functor  the callback to be invoked in the tick() method.
+    * @param ports         if your SimpleNode requires ports, provide the list here.
+    *
+    * */
+    void registerSimpleAction(const std::string& ID,
+                              const SimpleActionNode::TickFunctor& tick_functor,
+                              PortsList ports = {});
+    /**
+    * @brief registerSimpleCondition help you register nodes of type SimpleConditionNode.
+    *
+    * @param ID            registration ID
+    * @param tick_functor  the callback to be invoked in the tick() method.
+    * @param ports         if your SimpleNode requires ports, provide the list here.
+    *
+    * */
+    void registerSimpleCondition(const std::string& ID,
+                                 const SimpleConditionNode::TickFunctor& tick_functor,
+                                 PortsList ports = {});
+    /**
+    * @brief registerSimpleDecorator help you register nodes of type SimpleDecoratorNode.
+    *
+    * @param ID            registration ID
+    * @param tick_functor  the callback to be invoked in the tick() method.
+    * @param ports         if your SimpleNode requires ports, provide the list here.
+    *
+    * */
+    void registerSimpleDecorator(const std::string& ID,
+                                 const SimpleDecoratorNode::TickFunctor& tick_functor,
+                                 PortsList ports = {});
+
+    /**
+     * @brief registerFromPlugin load a shared library and execute the function BT_REGISTER_NODES (see macro).
+     *
+     * @param file_path path of the file
+     */
+    void registerFromPlugin(const std::string &file_path);
+
+    /**
+     * @brief registerFromROSPlugins finds all shared libraries that export ROS plugins for behaviortree_cpp, and calls registerFromPlugin for each library.
+     * @throws If not compiled with ROS support or if the library cannot load for any reason
+     *
+     */
+    void registerFromROSPlugins();
+
+    /**
+     * @brief instantiateTreeNode creates an instance of a previously registered TreeNode.
+     *
+     * @param name     name of this particular instance
+     * @param ID       ID used when it was registered
+     * @param config   configuration that is passed to the constructor of the TreeNode.
+     * @return         new node.
+     */
+    std::unique_ptr<TreeNode> instantiateTreeNode(const std::string& name, const std::string &ID,
+                                                  const NodeConfiguration& config) const;
+
+    /** registerNodeType is the method to use to register your custom TreeNode.
+     *
+     *  It accepts only classed derived from either ActionNodeBase, DecoratorNode,
+     *  ControlNode or ConditionNode.
+     */
+    template <typename T>
+    void registerNodeType(const std::string& ID)
+    {
+        static_assert(std::is_base_of<ActionNodeBase, T>::value ||
+                      std::is_base_of<ControlNode, T>::value ||
+                      std::is_base_of<DecoratorNode, T>::value ||
+                      std::is_base_of<ConditionNode, T>::value,
+                      "[registerNode]: accepts only classed derived from either ActionNodeBase, "
+                      "DecoratorNode, ControlNode or ConditionNode");
+
+        static_assert(!std::is_abstract<T>::value,
+                      "[registerNode]: Some methods are pure virtual. "
+                      "Did you override the methods tick() and halt()?");
+
+        constexpr bool default_constructable = std::is_constructible<T, const std::string&>::value;
+        constexpr bool param_constructable =
+                std::is_constructible<T, const std::string&, const NodeConfiguration&>::value;
+        constexpr bool has_static_ports_list =
+                has_static_method_providedPorts<T>::value;
+
+        static_assert(default_constructable || param_constructable,
+                      "[registerNode]: the registered class must have at least one of these two "
+                      "constructors: "
+                      "  (const std::string&, const NodeConfiguration&) or (const std::string&).");
+
+        static_assert(!(param_constructable && !has_static_ports_list),
+                      "[registerNode]: you MUST implement the static method: "
+                      "  PortsList providedPorts();\n");
+
+        static_assert(!(has_static_ports_list && !param_constructable),
+                      "[registerNode]: since you have a static method providedPorts(), "
+                      "you MUST add a constructor sign signature (const std::string&, const "
+                      "NodeParameters&)\n");
+
+        registerBuilder( CreateManifest<T>(ID), CreateBuilder<T>());
+    }
+
+    template <typename T>
+    void registerNodeType(const std::string& ID, PortsList ports)
+    {
+      static_assert(std::is_base_of<ActionNodeBase, T>::value ||
+                      std::is_base_of<ControlNode, T>::value ||
+                      std::is_base_of<DecoratorNode, T>::value ||
+                      std::is_base_of<ConditionNode, T>::value,
+                    "[registerNode]: accepts only classed derived from either ActionNodeBase, "
+                    "DecoratorNode, ControlNode or ConditionNode");
+
+      static_assert(!std::is_abstract<T>::value,
+                    "[registerNode]: Some methods are pure virtual. "
+                    "Did you override the methods tick() and halt()?");
+
+      constexpr bool default_constructable = std::is_constructible<T, const std::string&>::value;
+      constexpr bool param_constructable =
+        std::is_constructible<T, const std::string&, const NodeConfiguration&>::value;
+      constexpr bool has_static_ports_list =
+        has_static_method_providedPorts<T>::value;
+
+      static_assert(default_constructable || param_constructable,
+                    "[registerNode]: the registered class must have at least one of these two "
+                    "constructors: (const std::string&, const NodeConfiguration&) or (const std::string&).");
+
+      static_assert(!has_static_ports_list,
+                    "[registerNode]: ports are passed to this node explicitly. The static method"
+                    "providedPorts() should be removed to avoid ambiguities\n");
+
+      static_assert(param_constructable,
+                    "[registerNode]: since this node has ports, "
+                    "you MUST add a constructor sign signature (const std::string&, const "
+                    "NodeParameters&)\n");
+
+      registerBuilder( CreateManifest<T>(ID, ports), CreateBuilder<T>());
+    }
+
+    /// All the builders. Made available mostly for debug purposes.
+    const std::unordered_map<std::string, NodeBuilder>& builders() const;
+
+    /// Manifests of all the registered TreeNodes.
+    const std::unordered_map<std::string, TreeNodeManifest>& manifests() const;
+
+    /// List of builtin IDs.
+    const std::set<std::string>& builtinNodes() const;
+
+    Tree createTreeFromText(const std::string& text,
+                            Blackboard::Ptr blackboard = Blackboard::create());
+
+    Tree createTreeFromFile(const std::string& file_path,
+                            Blackboard::Ptr blackboard = Blackboard::create());
+
+private:
+    std::unordered_map<std::string, NodeBuilder> builders_;
+    std::unordered_map<std::string, TreeNodeManifest> manifests_;
+    std::set<std::string> builtin_IDs_;
+
+    // clang-format on
+};
+
+
+}   // end namespace
+
+#endif   // BT_FACTORY_H
diff --git a/include/behaviortree_cpp_v3/bt_parser.h b/include/behaviortree_cpp_v3/bt_parser.h
new file mode 100644 (file)
index 0000000..c5f9799
--- /dev/null
@@ -0,0 +1,33 @@
+#ifndef PARSING_BT_H
+#define PARSING_BT_H
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+#include "behaviortree_cpp_v3/blackboard.h"
+
+namespace BT
+{
+/**
+ * @brief The BehaviorTreeParser is a class used to read the model
+ * of a BehaviorTree from file or text and instantiate the
+ * corresponding tree using the BehaviorTreeFactory.
+ */
+class Parser
+{
+  public:
+    Parser() = default;
+
+    virtual ~Parser() = default;
+
+    Parser(const Parser& other) = delete;
+    Parser& operator=(const Parser& other) = delete;
+
+    virtual void loadFromFile(const std::string& filename) = 0;
+
+    virtual void loadFromText(const std::string& xml_text) = 0;
+
+    virtual Tree instantiateTree(const Blackboard::Ptr &root_blackboard) = 0;
+};
+
+}
+
+#endif   // PARSING_BT_H
diff --git a/include/behaviortree_cpp_v3/condition_node.h b/include/behaviortree_cpp_v3/condition_node.h
new file mode 100644 (file)
index 0000000..4dc11aa
--- /dev/null
@@ -0,0 +1,69 @@
+/* Copyright (C) 2015-2018 Michele Colledanchise -  All Rights Reserved
+ * Copyright (C) 2018-2020 Davide Faconti, Eurecat -  All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#ifndef CONDITIONNODE_H
+#define CONDITIONNODE_H
+
+#include "leaf_node.h"
+
+namespace BT
+{
+class ConditionNode : public LeafNode
+{
+  public:
+    ConditionNode(const std::string& name, const NodeConfiguration& config);
+
+    virtual ~ConditionNode() override = default;
+
+    //Do nothing
+    virtual void halt() override final
+    {
+        setStatus(NodeStatus::IDLE);
+    }
+
+    virtual NodeType type() const override final
+    {
+        return NodeType::CONDITION;
+    }
+};
+
+/**
+ * @brief The SimpleConditionNode provides an easy to use ConditionNode.
+ * The user should simply provide a callback with this signature
+ *
+ *    BT::NodeStatus functionName(void)
+ *
+ * This avoids the hassle of inheriting from a ActionNode.
+ *
+ * Using lambdas or std::bind it is easy to pass a pointer to a method.
+ * SimpleConditionNode does not support halting, NodeParameters, nor Blackboards.
+ */
+class SimpleConditionNode : public ConditionNode
+{
+  public:
+    typedef std::function<NodeStatus(TreeNode&)> TickFunctor;
+
+    // You must provide the function to call when tick() is invoked
+    SimpleConditionNode(const std::string& name, TickFunctor tick_functor,
+                        const NodeConfiguration& config);
+
+    ~SimpleConditionNode() override = default;
+
+  protected:
+    virtual NodeStatus tick() override;
+
+    TickFunctor tick_functor_;
+};
+}
+
+#endif
diff --git a/include/behaviortree_cpp_v3/control_node.h b/include/behaviortree_cpp_v3/control_node.h
new file mode 100644 (file)
index 0000000..558b788
--- /dev/null
@@ -0,0 +1,60 @@
+/* Copyright (C) 2015-2018 Michele Colledanchise -  All Rights Reserved
+ * Copyright (C) 2018-2020 Davide Faconti, Eurecat -  All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#ifndef CONTROLNODE_H
+#define CONTROLNODE_H
+
+#include <vector>
+#include "behaviortree_cpp_v3/tree_node.h"
+
+namespace BT
+{
+class ControlNode : public TreeNode
+{
+  protected:
+    std::vector<TreeNode*> children_nodes_;
+
+  public:
+    ControlNode(const std::string& name, const NodeConfiguration& config);
+
+    virtual ~ControlNode() override = default;
+
+    /// The method used to add nodes to the children vector
+    void addChild(TreeNode* child);
+
+    size_t childrenCount() const;
+
+    const std::vector<TreeNode*>& children() const;
+
+    const TreeNode* child(size_t index) const
+    {
+        return children().at(index);
+    }
+
+    virtual void halt() override;
+
+    void haltChildren();
+
+    [[deprecated( "deprecated: please use explicitly haltChildren() or haltChild(i)")]]
+    void haltChildren(size_t first);
+
+    void haltChild(size_t i);
+
+    virtual NodeType type() const override final
+    {
+        return NodeType::CONTROL;
+    }
+};
+}
+
+#endif
diff --git a/include/behaviortree_cpp_v3/controls/fallback_node.h b/include/behaviortree_cpp_v3/controls/fallback_node.h
new file mode 100644 (file)
index 0000000..5492c8b
--- /dev/null
@@ -0,0 +1,50 @@
+/* Copyright (C) 2015-2018 Michele Colledanchise -  All Rights Reserved
+ * Copyright (C) 2018-2020 Davide Faconti, Eurecat -  All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#ifndef FALLBACKNODE_H
+#define FALLBACKNODE_H
+
+#include "behaviortree_cpp_v3/control_node.h"
+
+namespace BT
+{
+/**
+ * @brief The FallbackNode is used to try different strategies,
+ * until one succeeds.
+ * If any child returns RUNNING, previous children will NOT be ticked again.
+ *
+ * - If all the children return FAILURE, this node returns FAILURE.
+ *
+ * - If a child returns RUNNING, this node returns RUNNING.
+ *
+ * - If a child returns SUCCESS, stop the loop and return SUCCESS.
+ *
+ */
+class FallbackNode : public ControlNode
+{
+  public:
+    FallbackNode(const std::string& name);
+
+    virtual ~FallbackNode() override = default;
+
+    virtual void halt() override;
+
+  private:
+    size_t current_child_idx_;
+
+    virtual BT::NodeStatus tick() override;
+};
+
+}
+
+#endif
diff --git a/include/behaviortree_cpp_v3/controls/if_then_else_node.h b/include/behaviortree_cpp_v3/controls/if_then_else_node.h
new file mode 100644 (file)
index 0000000..2e1ab40
--- /dev/null
@@ -0,0 +1,52 @@
+/* Copyright (C) 2020 Davide Faconti -  All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#ifndef BT_IF_THEN_ELSE_H
+#define BT_IF_THEN_ELSE_H
+
+#include "behaviortree_cpp_v3/control_node.h"
+
+namespace BT
+{
+/**
+ * @brief IfThenElseNode must have exactly 2 or 3 children. This node is NOT reactive.
+ *
+ * The first child is the "statement" of the if.
+ *
+ * If that return SUCCESS, then the second child is executed.
+ *
+ * Instead, if it returned FAILURE, the third child is executed.
+ *
+ * If you have only 2 children, this node will return FAILURE whenever the
+ * statement returns FAILURE.
+ *
+ * This is equivalent to add AlwaysFailure as 3rd child.
+ *
+ */
+class IfThenElseNode : public ControlNode
+{
+  public:
+    IfThenElseNode(const std::string& name);
+
+    virtual ~IfThenElseNode() override = default;
+
+    virtual void halt() override;
+
+  private:
+    size_t child_idx_;
+
+    virtual BT::NodeStatus tick() override;
+};
+
+}
+
+#endif // BT_IF_THEN_ELSE_H
diff --git a/include/behaviortree_cpp_v3/controls/manual_node.h b/include/behaviortree_cpp_v3/controls/manual_node.h
new file mode 100644 (file)
index 0000000..895385b
--- /dev/null
@@ -0,0 +1,59 @@
+/* Copyright (C) 2020 Davide Faconti -  All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#ifndef MANUAL_SELECTION_NODE_H
+#define MANUAL_SELECTION_NODE_H
+
+#include "behaviortree_cpp_v3/control_node.h"
+
+namespace BT
+{
+/**
+ * @brief Use a Terminal User Interface (ncurses) to select a certain child manually.
+ */
+class ManualSelectorNode : public ControlNode
+{
+  public:
+    ManualSelectorNode(const std::string& name, const NodeConfiguration& config);
+
+    virtual ~ManualSelectorNode() override = default;
+
+    virtual void halt() override;
+
+    static PortsList providedPorts()
+    {
+        return { InputPort<bool>(REPEAT_LAST_SELECTION, false,
+                                 "If true, execute again the same child that was selected the last time") };
+    }
+
+  private:
+
+    static constexpr const char* REPEAT_LAST_SELECTION = "repeat_last_selection";
+
+    virtual BT::NodeStatus tick() override;
+    int running_child_idx_;
+    int previously_executed_idx_;
+
+    enum NumericarStatus{
+        NUM_SUCCESS = 253,
+        NUM_FAILURE = 254,
+        NUM_RUNNING = 255,
+    };
+
+    NodeStatus selectStatus() const;
+
+    uint8_t selectChild() const;
+};
+
+}
+
+#endif // MANUAL_SELECTION_NODE_H
diff --git a/include/behaviortree_cpp_v3/controls/parallel_node.h b/include/behaviortree_cpp_v3/controls/parallel_node.h
new file mode 100644 (file)
index 0000000..b5a5ffb
--- /dev/null
@@ -0,0 +1,61 @@
+/* Copyright (C) 2015-2018 Michele Colledanchise -  All Rights Reserved
+ * Copyright (C) 2018-2020 Davide Faconti, Eurecat -  All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#ifndef PARALLEL_NODE_H
+#define PARALLEL_NODE_H
+
+#include <set>
+#include "behaviortree_cpp_v3/control_node.h"
+
+namespace BT
+{
+
+class ParallelNode : public ControlNode
+{
+  public:
+
+    ParallelNode(const std::string& name, unsigned success_threshold,
+                 unsigned failure_threshold = 1);
+
+    ParallelNode(const std::string& name, const NodeConfiguration& config);
+
+    static PortsList providedPorts()
+    {
+        return { InputPort<unsigned>(THRESHOLD_SUCCESS, "number of childen which need to succeed to trigger a SUCCESS" ),
+                 InputPort<unsigned>(THRESHOLD_FAILURE, 1, "number of childen which need to fail to trigger a FAILURE" ) };
+    }
+
+    ~ParallelNode() = default;
+
+    virtual void halt() override;
+
+    unsigned int thresholdM();
+    unsigned int thresholdFM();
+    void setThresholdM(unsigned int threshold_M);
+    void setThresholdFM(unsigned int threshold_M);
+
+  private:
+    unsigned int success_threshold_;
+    unsigned int failure_threshold_;
+
+    std::set<int> skip_list_;
+
+    bool read_parameter_from_ports_;
+    static constexpr const char* THRESHOLD_SUCCESS = "success_threshold";
+    static constexpr const char* THRESHOLD_FAILURE = "failure_threshold";
+
+    virtual BT::NodeStatus tick() override;
+};
+
+}
+#endif   // PARALLEL_NODE_H
diff --git a/include/behaviortree_cpp_v3/controls/reactive_fallback.h b/include/behaviortree_cpp_v3/controls/reactive_fallback.h
new file mode 100644 (file)
index 0000000..14b453c
--- /dev/null
@@ -0,0 +1,48 @@
+/* Copyright (C) 2020 Davide Faconti, Eurecat -  All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#ifndef REACTIVE_FALLBACK_NODE_H
+#define REACTIVE_FALLBACK_NODE_H
+
+#include "behaviortree_cpp_v3/control_node.h"
+
+namespace BT
+{
+
+/**
+ * @brief The ReactiveFallback is similar to a ParallelNode.
+ * All the children are ticked from first to last:
+ *
+ * - If a child returns RUNNING, continue to the next sibling.
+ * - If a child returns FAILURE, continue to the next sibling.
+ * - If a child returns SUCCESS, stop and return SUCCESS.
+ *
+ * If all the children fail, than this node returns FAILURE.
+ *
+ * IMPORTANT: to work properly, this node should not have more than
+ *            a single asynchronous child.
+ *
+ */
+class ReactiveFallback : public ControlNode
+{
+  public:
+
+    ReactiveFallback(const std::string& name):
+      ControlNode(name, {}){}
+
+  private:
+
+    virtual BT::NodeStatus tick() override;
+};
+
+}
+#endif   // REACTIVE_FALLBACK_NODE_H
diff --git a/include/behaviortree_cpp_v3/controls/reactive_sequence.h b/include/behaviortree_cpp_v3/controls/reactive_sequence.h
new file mode 100644 (file)
index 0000000..d621f97
--- /dev/null
@@ -0,0 +1,47 @@
+/* Copyright (C) 2020 Davide Faconti, Eurecat -  All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#ifndef REACTIVE_SEQUENCE_NODE_H
+#define REACTIVE_SEQUENCE_NODE_H
+
+#include "behaviortree_cpp_v3/control_node.h"
+
+namespace BT
+{
+/**
+ * @brief The ReactiveSequence is similar to a ParallelNode.
+ * All the children are ticked from first to last:
+ *
+ * - If a child returns RUNNING, tick the next sibling.
+ * - If a child returns SUCCESS, tick the next sibling.
+ * - If a child returns FAILURE, stop and return FAILURE.
+ *
+ * If all the children return SUCCESS, this node returns SUCCESS.
+ *
+ * IMPORTANT: to work properly, this node should not have more than a single
+ *            asynchronous child.
+ *
+ */
+class ReactiveSequence : public ControlNode
+{
+  public:
+
+    ReactiveSequence(const std::string& name):
+        ControlNode(name, {}) {}
+
+  private:
+
+    virtual BT::NodeStatus tick() override;
+};
+
+}
+#endif  // REACTIVE_SEQUENCE_NODE_H
diff --git a/include/behaviortree_cpp_v3/controls/sequence_node.h b/include/behaviortree_cpp_v3/controls/sequence_node.h
new file mode 100644 (file)
index 0000000..20baadc
--- /dev/null
@@ -0,0 +1,51 @@
+/* Copyright (C) 2015-2018 Michele Colledanchise -  All Rights Reserved
+ * Copyright (C) 2018-2020 Davide Faconti, Eurecat -  All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#ifndef SEQUENCENODE_H
+#define SEQUENCENODE_H
+
+#include "behaviortree_cpp_v3/control_node.h"
+
+namespace BT
+{
+/**
+ * @brief The SequenceNode is used to tick children in an ordered sequence.
+ * If any child returns RUNNING, previous children will NOT be ticked again.
+ *
+ * - If all the children return SUCCESS, this node returns SUCCESS.
+ *
+ * - If a child returns RUNNING, this node returns RUNNING.
+ *   Loop is NOT restarted, the same running child will be ticked again.
+ *
+ * - If a child returns FAILURE, stop the loop and return FAILURE.
+ *   Restart the loop only if (reset_on_failure == true)
+ *
+ */
+class SequenceNode : public ControlNode
+{
+  public:
+    SequenceNode(const std::string& name);
+
+    virtual ~SequenceNode() override = default;
+
+    virtual void halt() override;
+
+  private:
+    size_t current_child_idx_;
+
+    virtual BT::NodeStatus tick() override;
+};
+
+}
+
+#endif // SEQUENCENODE_H
diff --git a/include/behaviortree_cpp_v3/controls/sequence_star_node.h b/include/behaviortree_cpp_v3/controls/sequence_star_node.h
new file mode 100644 (file)
index 0000000..611a26f
--- /dev/null
@@ -0,0 +1,53 @@
+/* Copyright (C) 2015-2018 Michele Colledanchise -  All Rights Reserved
+ * Copyright (C) 2018-2020 Davide Faconti, Eurecat -  All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#ifndef SEQUENCE_NODE_WITH_MEMORY_H
+#define SEQUENCE_NODE_WITH_MEMORY_H
+
+#include "behaviortree_cpp_v3/control_node.h"
+
+namespace BT
+{
+/**
+ * @brief The SequenceStarNode is used to tick children in an ordered sequence.
+ * If any child returns RUNNING, previous children are not ticked again.
+ *
+ * - If all the children return SUCCESS, this node returns SUCCESS.
+ *
+ * - If a child returns RUNNING, this node returns RUNNING.
+ *   Loop is NOT restarted, the same running child will be ticked again.
+ *
+ * - If a child returns FAILURE, stop the loop and return FAILURE.
+ *   Loop is NOT restarted, the same running child will be ticked again.
+ *
+ */
+
+class SequenceStarNode : public ControlNode
+{
+  public:
+    SequenceStarNode(const std::string& name);
+
+    virtual ~SequenceStarNode() override = default;
+
+    virtual void halt() override;
+
+  private:
+
+    size_t current_child_idx_;
+
+    virtual BT::NodeStatus tick() override;
+};
+
+}
+
+#endif   // SEQUENCE_NODE_WITH_MEMORY_H
diff --git a/include/behaviortree_cpp_v3/controls/switch_node.h b/include/behaviortree_cpp_v3/controls/switch_node.h
new file mode 100644 (file)
index 0000000..5095bf8
--- /dev/null
@@ -0,0 +1,141 @@
+/* Copyright (C) 2020 Davide Faconti -  All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#ifndef SWITCH_NODE_H
+#define SWITCH_NODE_H
+
+#include "behaviortree_cpp_v3/control_node.h"
+
+namespace BT
+{
+/**
+ * @brief The SwitchNode is equivalent to a switch statement, where a certain
+ * branch (child) is executed according to the value of a blackboard entry.
+ *
+ * Note that the same behaviour can be achieved with multiple Sequences, Fallbacks and
+ * Conditions reading the blackboard, but switch is shorter and more readable.
+ *
+ * Example usage:
+ *
+
+<Switch3 variable="{var}"  case_1="1" case_2="42" case_3="666" >
+   <ActionA name="action_when_var_eq_1" />
+   <ActionB name="action_when_var_eq_42" />
+   <ActionC name="action_when_var_eq_666" />
+   <ActionD name="default_action" />
+ </Switch3>
+
+When the SwitchNode is executed (Switch3 is a node with 3 cases)
+the "variable" will be compared to the cases and execute the correct child
+or the default one (last).
+
+ *
+ */
+template <size_t NUM_CASES>
+class SwitchNode : public ControlNode
+{
+  public:
+    SwitchNode(const std::string& name, const BT::NodeConfiguration& config)
+    : ControlNode::ControlNode(name, config ),
+      running_child_(-1)
+    {
+        setRegistrationID("Switch");
+    }
+
+    virtual ~SwitchNode() override = default;
+
+    void halt() override
+    {
+        running_child_ = -1;
+        ControlNode::halt();
+    }
+
+    static PortsList providedPorts()
+    {
+        PortsList ports;
+        ports.insert( BT::InputPort<std::string>("variable") );
+        for(unsigned i=0; i < NUM_CASES; i++)
+        {
+            char case_str[20];
+            sprintf(case_str, "case_%d", i+1);
+            ports.insert( BT::InputPort<std::string>(case_str) );
+        }
+        return ports;
+    }
+
+  private:
+    int running_child_;
+    virtual BT::NodeStatus tick() override;
+};
+
+template<size_t NUM_CASES> inline
+NodeStatus SwitchNode<NUM_CASES>::tick()
+{
+    constexpr const char * case_port_names[9] = {
+      "case_1", "case_2", "case_3", "case_4", "case_5", "case_6", "case_7", "case_8", "case_9"};
+
+    if( childrenCount() != NUM_CASES+1)
+    {
+        throw LogicError("Wrong number of children in SwitchNode; "
+                         "must be (num_cases + default)");
+    }
+
+    std::string variable;
+    std::string value;
+    int child_index = NUM_CASES; // default index;
+
+    if (getInput("variable", variable)) // no variable? jump to default
+    {
+        // check each case until you find a match
+        for (unsigned index = 0; index < NUM_CASES; ++index)
+        {
+            bool found = false;
+            if( index < 9 )
+            {
+                found = (bool)getInput(case_port_names[index], value);
+            }
+            else{
+                char case_str[20];
+                sprintf(case_str, "case_%d", index+1);
+                found = (bool)getInput(case_str, value);
+            }
+
+            if (found && variable == value)
+            {
+                child_index = index;
+                break;
+            }
+        }
+    }
+
+    // if another one was running earlier, halt it
+    if( running_child_ != -1 && running_child_ != child_index)
+    {
+        haltChild(running_child_);
+    }
+
+    auto& selected_child = children_nodes_[child_index];
+    NodeStatus ret = selected_child->executeTick();
+    if( ret == NodeStatus::RUNNING )
+    {
+        running_child_ = child_index;
+    }
+    else{
+        haltChildren();
+        running_child_ = -1;
+    }
+    return ret;
+}
+
+}
+
+#endif // SWITCH_NODE_H
diff --git a/include/behaviortree_cpp_v3/controls/while_do_else_node.h b/include/behaviortree_cpp_v3/controls/while_do_else_node.h
new file mode 100644 (file)
index 0000000..21d393a
--- /dev/null
@@ -0,0 +1,50 @@
+/* Copyright (C) 2020 Davide Faconti -  All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#ifndef BT_WHILE_DO_ELSE_H
+#define BT_WHILE_DO_ELSE_H
+
+#include "behaviortree_cpp_v3/control_node.h"
+
+namespace BT
+{
+/**
+ * @brief WhileDoElse must have exactly 2 or 3 children.
+ * It is a REACTIVE node of IfThenElseNode.
+ *
+ * The first child is the "statement" that is executed at each tick
+ *
+ * If result is SUCCESS, the second child is executed.
+ *
+ * If result is FAILURE, the third child is executed.
+ *
+ * If the 2nd or 3d child is RUNNING and the statement changes,
+ * the RUNNING child will be stopped before starting the sibling.
+ *
+ */
+class WhileDoElseNode : public ControlNode
+{
+  public:
+    WhileDoElseNode(const std::string& name);
+
+    virtual ~WhileDoElseNode() override = default;
+
+    virtual void halt() override;
+
+  private:
+
+    virtual BT::NodeStatus tick() override;
+};
+
+}
+
+#endif // BT_WHILE_DO_ELSE_H
diff --git a/include/behaviortree_cpp_v3/decorator_node.h b/include/behaviortree_cpp_v3/decorator_node.h
new file mode 100644 (file)
index 0000000..a6f2558
--- /dev/null
@@ -0,0 +1,67 @@
+#ifndef DECORATORNODE_H
+#define DECORATORNODE_H
+
+#include "behaviortree_cpp_v3/tree_node.h"
+
+namespace BT
+{
+class DecoratorNode : public TreeNode
+{
+  protected:
+    TreeNode* child_node_;
+
+  public:
+
+    DecoratorNode(const std::string& name, const NodeConfiguration& config);
+
+    virtual ~DecoratorNode() override = default;
+
+    void setChild(TreeNode* child);
+
+    const TreeNode* child() const;
+
+    TreeNode* child();
+
+    /// The method used to interrupt the execution of this node
+    virtual void halt() override;
+
+    /// Halt() the child node
+    void haltChild();
+
+    virtual NodeType type() const override
+    {
+        return NodeType::DECORATOR;
+    }
+
+    NodeStatus executeTick() override;
+};
+
+/**
+ * @brief The SimpleDecoratorNode provides an easy to use DecoratorNode.
+ * The user should simply provide a callback with this signature
+ *
+ *    BT::NodeStatus functionName(BT::NodeStatus child_status)
+ *
+ * This avoids the hassle of inheriting from a DecoratorNode.
+ *
+ * Using lambdas or std::bind it is easy to pass a pointer to a method.
+ * SimpleDecoratorNode does not support halting, NodeParameters, nor Blackboards.
+ */
+class SimpleDecoratorNode : public DecoratorNode
+{
+  public:
+    typedef std::function<NodeStatus(NodeStatus, TreeNode&)> TickFunctor;
+
+    // You must provide the function to call when tick() is invoked
+    SimpleDecoratorNode(const std::string& name, TickFunctor tick_functor, const NodeConfiguration& config);
+
+    ~SimpleDecoratorNode() override = default;
+
+  protected:
+    virtual NodeStatus tick() override;
+
+    TickFunctor tick_functor_;
+};
+}
+
+#endif
diff --git a/include/behaviortree_cpp_v3/decorators/blackboard_precondition.h b/include/behaviortree_cpp_v3/decorators/blackboard_precondition.h
new file mode 100644 (file)
index 0000000..4cc445d
--- /dev/null
@@ -0,0 +1,88 @@
+/*  Copyright (C) 2018-2020 Davide Faconti, Eurecat -  All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#ifndef DECORATOR_BLACKBOARD_PRECONDITION_NODE_H
+#define DECORATOR_BLACKBOARD_PRECONDITION_NODE_H
+
+#include "behaviortree_cpp_v3/decorator_node.h"
+
+namespace BT
+{
+/**
+ * This node excute its child only if the value of a given input port
+ * is equal to the expected one.
+ * If this precondition is met, this node will return the same status of the
+ * child, otherwise it will return the value specified in "return_on_mismatch".
+ *
+ * Example:
+ *
+ * <BlackboardCheckInt value_A="{the_answer}"
+ *                     value_B="42"
+ *                     return_on_mismatch="FAILURE" />
+ */
+template <typename T>
+class BlackboardPreconditionNode : public DecoratorNode
+{
+  public:
+    BlackboardPreconditionNode(const std::string& name, const NodeConfiguration& config)
+      : DecoratorNode(name, config)
+    {
+        if( std::is_same<T,int>::value)
+            setRegistrationID("BlackboardCheckInt");
+        else if( std::is_same<T,double>::value)
+            setRegistrationID("BlackboardCheckDouble");
+        else if( std::is_same<T,std::string>::value)
+            setRegistrationID("BlackboardCheckString");
+    }
+
+    virtual ~BlackboardPreconditionNode() override = default;
+
+    static PortsList providedPorts()
+    {
+        return {InputPort("value_A"),
+                InputPort("value_B"),
+                InputPort<NodeStatus>("return_on_mismatch") };
+    }
+
+  private:
+    virtual BT::NodeStatus tick() override;
+};
+
+//----------------------------------------------------
+
+template<typename T> inline
+NodeStatus BlackboardPreconditionNode<T>::tick()
+{
+    T value_A;
+    T value_B;
+    NodeStatus default_return_status = NodeStatus::FAILURE;
+
+    setStatus(NodeStatus::RUNNING);
+
+    if( getInput("value_A", value_A) &&
+        getInput("value_B", value_B) &&
+        value_B == value_A )
+    {
+        return child_node_->executeTick();
+    }
+
+    if( child()->status() == NodeStatus::RUNNING )
+    {
+        haltChild();
+    }
+    getInput("return_on_mismatch", default_return_status);
+    return default_return_status;
+}
+
+} // end namespace
+
+#endif
diff --git a/include/behaviortree_cpp_v3/decorators/delay_node.h b/include/behaviortree_cpp_v3/decorators/delay_node.h
new file mode 100644 (file)
index 0000000..cf5392a
--- /dev/null
@@ -0,0 +1,62 @@
+#ifndef DECORATOR_DELAY_NODE_H
+#define DECORATOR_DELAY_NODE_H
+
+#include "behaviortree_cpp_v3/decorator_node.h"
+#include <atomic>
+#include "timer_queue.h"
+
+namespace BT
+{
+/**
+ * @brief The delay node will introduce a delay of a few milliseconds
+ * and then tick the child returning the status of the child as it is 
+ * upon completion
+ * The delay is in milliseconds and it is passed using the port "delay_msec".
+ *
+ * During the delay the node changes status to RUNNING
+ *
+ * Example:
+ *
+ * <Delay delay_msec="5000">
+ *    <KeepYourBreath/>
+ * </Delay>
+ */
+class DelayNode : public DecoratorNode
+{
+  public:
+    DelayNode(const std::string& name, unsigned milliseconds);
+
+    DelayNode(const std::string& name, const NodeConfiguration& config);
+
+    ~DelayNode() override
+    {
+        halt();
+    }
+
+    static PortsList providedPorts()
+    {
+        return {InputPort<unsigned>("delay_msec", "Tick the child after a few milliseconds")};
+    }
+    void halt() override
+    {
+        timer_.cancelAll();
+        DecoratorNode::halt();
+    }
+
+  private:
+    TimerQueue<> timer_;
+    uint64_t timer_id_;
+
+    virtual BT::NodeStatus tick() override;
+
+    bool delay_started_;
+    bool delay_complete_;
+    bool delay_aborted_;
+    unsigned msec_;
+    bool read_parameter_from_ports_;
+    std::mutex delay_mutex_;
+};
+
+}   // namespace BT
+
+#endif   // DELAY_NODE_H
diff --git a/include/behaviortree_cpp_v3/decorators/force_failure_node.h b/include/behaviortree_cpp_v3/decorators/force_failure_node.h
new file mode 100644 (file)
index 0000000..532bedd
--- /dev/null
@@ -0,0 +1,66 @@
+/*  Copyright (C) 2018-2020 Davide Faconti, Eurecat -  All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#ifndef DECORATOR_ALWAYS_FAILURE_NODE_H
+#define DECORATOR_ALWAYS_FAILURE_NODE_H
+
+#include "behaviortree_cpp_v3/decorator_node.h"
+
+namespace BT
+{
+/**
+ * @brief The ForceFailureNode returns always FAILURE or RUNNING.
+ */
+class ForceFailureNode : public DecoratorNode
+{
+  public:
+    ForceFailureNode(const std::string& name) :
+        DecoratorNode(name, {} )
+    {
+        setRegistrationID("ForceFailure");
+    }
+
+  private:
+    virtual BT::NodeStatus tick() override;
+};
+
+//------------ implementation ----------------------------
+
+inline NodeStatus ForceFailureNode::tick()
+{
+    setStatus(NodeStatus::RUNNING);
+
+    const NodeStatus child_state = child_node_->executeTick();
+
+    switch (child_state)
+    {
+        case NodeStatus::FAILURE:
+        case NodeStatus::SUCCESS:
+        {
+            return NodeStatus::FAILURE;
+        }
+
+        case NodeStatus::RUNNING:
+        {
+            return NodeStatus::RUNNING;
+        }
+
+        default:
+        {
+            // TODO throw?
+        }
+    }
+    return status();
+}
+}
+
+#endif
diff --git a/include/behaviortree_cpp_v3/decorators/force_success_node.h b/include/behaviortree_cpp_v3/decorators/force_success_node.h
new file mode 100644 (file)
index 0000000..699d23f
--- /dev/null
@@ -0,0 +1,66 @@
+/*  Copyright (C) 2018-2020 Davide Faconti, Eurecat -  All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#ifndef DECORATOR_ALWAYS_SUCCESS_NODE_H
+#define DECORATOR_ALWAYS_SUCCESS_NODE_H
+
+#include "behaviortree_cpp_v3/decorator_node.h"
+
+namespace BT
+{
+/**
+ * @brief The ForceSuccessNode returns always SUCCESS or RUNNING.
+ */
+class ForceSuccessNode : public DecoratorNode
+{
+  public:
+    ForceSuccessNode(const std::string& name) :
+        DecoratorNode(name, {} )
+    {
+        setRegistrationID("ForceSuccess");
+    }
+
+  private:
+    virtual BT::NodeStatus tick() override;
+};
+
+//------------ implementation ----------------------------
+
+inline NodeStatus ForceSuccessNode::tick()
+{
+    setStatus(NodeStatus::RUNNING);
+
+    const NodeStatus child_state = child_node_->executeTick();
+
+    switch (child_state)
+    {
+        case NodeStatus::FAILURE:
+        case NodeStatus::SUCCESS:
+        {
+            return NodeStatus::SUCCESS;
+        }
+
+        case NodeStatus::RUNNING:
+        {
+            return NodeStatus::RUNNING;
+        }
+
+        default:
+        {
+            // TODO throw?
+        }
+    }
+    return status();
+}
+}
+
+#endif
diff --git a/include/behaviortree_cpp_v3/decorators/inverter_node.h b/include/behaviortree_cpp_v3/decorators/inverter_node.h
new file mode 100644 (file)
index 0000000..4d4a103
--- /dev/null
@@ -0,0 +1,38 @@
+/* Copyright (C) 2018 Michele Colledanchise -  All Rights Reserved
+ * Copyright (C) 2018-2020 Davide Faconti, Eurecat -  All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#ifndef DECORATOR_INVERTER_NODE_H
+#define DECORATOR_INVERTER_NODE_H
+
+#include "behaviortree_cpp_v3/decorator_node.h"
+
+namespace BT
+{
+/**
+ * @brief The InverterNode returns SUCCESS if child fails
+ * of FAILURE is child succeeds.
+ * RUNNING status is propagated
+ */
+class InverterNode : public DecoratorNode
+{
+  public:
+    InverterNode(const std::string& name);
+
+    virtual ~InverterNode() override = default;
+
+  private:
+    virtual BT::NodeStatus tick() override;
+};
+}
+
+#endif
diff --git a/include/behaviortree_cpp_v3/decorators/keep_running_until_failure_node.h b/include/behaviortree_cpp_v3/decorators/keep_running_until_failure_node.h
new file mode 100644 (file)
index 0000000..af9c88f
--- /dev/null
@@ -0,0 +1,66 @@
+/*  Copyright (C) 2018-2020 Davide Faconti, Eurecat -  All Rights Reserved
+*   Copyright (C) 2020 Francisco Martin, Intelligent Robotics Lab (URJC) <fmrico@gmail.com>
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#ifndef DECORATOR_KEEP_RUNNING_UNTIL_FAILURE_H
+#define DECORATOR_KEEP_RUNNING_UNTIL_FAILURE_H
+
+#include "behaviortree_cpp_v3/decorator_node.h"
+
+namespace BT
+{
+/**
+ * @brief The KeepRunningUntilFailureNode returns always FAILURE or RUNNING.
+ */
+class KeepRunningUntilFailureNode : public DecoratorNode
+{
+  public:
+    KeepRunningUntilFailureNode(const std::string& name) :
+        DecoratorNode(name, {} )
+    {
+        setRegistrationID("KeepRunningUntilFailure");
+    }
+
+  private:
+    virtual BT::NodeStatus tick() override;
+};
+
+//------------ implementation ----------------------------
+
+inline NodeStatus KeepRunningUntilFailureNode::tick()
+{
+    setStatus(NodeStatus::RUNNING);
+
+    const NodeStatus child_state = child_node_->executeTick();
+
+    switch (child_state)
+    {
+        case NodeStatus::FAILURE:
+        {
+            return NodeStatus::FAILURE;
+        }    
+        case NodeStatus::SUCCESS:
+        case NodeStatus::RUNNING:
+        {
+            return NodeStatus::RUNNING;
+        }
+
+        default:
+        {
+            // TODO throw?
+        }
+    }
+    return status();
+}
+}
+
+#endif
diff --git a/include/behaviortree_cpp_v3/decorators/repeat_node.h b/include/behaviortree_cpp_v3/decorators/repeat_node.h
new file mode 100644 (file)
index 0000000..35c4594
--- /dev/null
@@ -0,0 +1,67 @@
+/* Copyright (C) 2015-2018 Michele Colledanchise -  All Rights Reserved
+ * Copyright (C) 2018-2020 Davide Faconti, Eurecat -  All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#ifndef DECORATOR_REPEAT_NODE_H
+#define DECORATOR_REPEAT_NODE_H
+
+#include "behaviortree_cpp_v3/decorator_node.h"
+
+namespace BT
+{
+/**
+ * @brief The RepeatNode is used to execute a child several times, as long
+ * as it succeed.
+ *
+ * To succeed, the child must return SUCCESS N times (port "num_cycles").
+ *
+ * If the child returns FAILURE, the loop is stopped and this node
+ * returns FAILURE.
+ *
+ * Example:
+ *
+ * <Repeat num_cycles="3">
+ *   <ClapYourHandsOnce/>
+ * </Repeat>
+ */
+class RepeatNode : public DecoratorNode
+{
+  public:
+
+    RepeatNode(const std::string& name, int NTries);
+
+    RepeatNode(const std::string& name, const NodeConfiguration& config);
+
+    virtual ~RepeatNode() override = default;
+
+    static PortsList providedPorts()
+    {
+        return { InputPort<int>(NUM_CYCLES,
+                                "Repeat a succesful child up to N times. "
+                               "Use -1 to create an infinite loop.") };
+    }
+
+  private:
+    int num_cycles_;
+    int try_index_;
+
+    bool read_parameter_from_ports_;
+    static constexpr const char* NUM_CYCLES = "num_cycles";
+
+    virtual NodeStatus tick() override;
+
+    void halt() override;
+};
+
+}
+
+#endif
diff --git a/include/behaviortree_cpp_v3/decorators/retry_node.h b/include/behaviortree_cpp_v3/decorators/retry_node.h
new file mode 100644 (file)
index 0000000..60c4e24
--- /dev/null
@@ -0,0 +1,66 @@
+/* Copyright (C) 2015-2018 Michele Colledanchise -  All Rights Reserved
+ * Copyright (C) 2018-2020 Davide Faconti, Eurecat -  All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#ifndef DECORATORRETRYNODE_H
+#define DECORATORRETRYNODE_H
+
+#include "behaviortree_cpp_v3/decorator_node.h"
+
+namespace BT
+{
+/**
+ * @brief The RetryNode is used to execute a child several times if it fails.
+ *
+ * If the child returns SUCCESS, the loop is stopped and this node
+ * returns SUCCESS.
+ *
+ * If the child returns FAILURE, this node will try again up to N times
+ * (N is read from port "num_attempts").
+ *
+ * Example:
+ *
+ * <RetryUntilSuccesful num_attempts="3">
+ *     <OpenDoor/>
+ * </RetryUntilSuccesful>
+ */
+class RetryNode : public DecoratorNode
+{
+  public:
+    
+    RetryNode(const std::string& name, int NTries);
+
+    RetryNode(const std::string& name, const NodeConfiguration& config);
+
+    virtual ~RetryNode() override = default;
+
+    static PortsList providedPorts()
+    {
+        return { InputPort<int>(NUM_ATTEMPTS,
+                               "Execute again a failing child up to N times. "
+                               "Use -1 to create an infinite loop.") };
+    }
+
+    virtual void halt() override;
+
+  private:
+    int max_attempts_;
+    int try_index_;
+
+    bool read_parameter_from_ports_;
+    static constexpr const char* NUM_ATTEMPTS = "num_attempts";
+
+    virtual BT::NodeStatus tick() override;
+};
+}
+
+#endif
diff --git a/include/behaviortree_cpp_v3/decorators/subtree_node.h b/include/behaviortree_cpp_v3/decorators/subtree_node.h
new file mode 100644 (file)
index 0000000..f9ae66d
--- /dev/null
@@ -0,0 +1,106 @@
+#ifndef DECORATOR_SUBTREE_NODE_H
+#define DECORATOR_SUBTREE_NODE_H
+
+#include "behaviortree_cpp_v3/decorator_node.h"
+
+namespace BT
+{
+
+/**
+ * @brief The SubtreeNode is a way to wrap an entire Subtree,
+ * creating a separated BlackBoard.
+ * If you want to have data flow through ports, you need to explicitly
+ * remap the ports.
+ */
+class SubtreeNode : public DecoratorNode
+{
+  public:
+    SubtreeNode(const std::string& name);
+
+    virtual ~SubtreeNode() override = default;
+
+  private:
+    virtual BT::NodeStatus tick() override;
+
+    static PortsList providedPorts()
+    {
+        return { InputPort<bool>("__shared_blackboard", false,
+                                 "If false (default) the subtree has its own blackboard and you"
+                                 "need to do port remapping to connect it to the parent") };
+    }
+
+    virtual NodeType type() const override final
+    {
+        return NodeType::SUBTREE;
+    }
+};
+
+
+
+/**
+ * @brief The SubtreePlus is a new kind of subtree that gives you much more control over remapping:
+ *
+ * Consider this example:
+
+<root main_tree_to_execute = "MainTree" >
+
+    <BehaviorTree ID="MainTree">
+        <Sequence>
+
+        <SetBlackboard value="Hello" output_key="myParam" />
+        <SubTreePlus ID="Talk" param="{myParam}" />
+
+        <SubTreePlus ID="Talk" param="World" />
+
+        <SetBlackboard value="Auto remapped" output_key="param" />
+        <SubTreePlus ID="Talk" __autoremap="1"  />
+
+        </Sequence>
+    </BehaviorTree>
+
+    <BehaviorTree ID="Talk">
+        <SaySomething message="{param}" />
+    </BehaviorTree>
+</root>
+
+ * You may notice three different approaches to remapping:
+ *
+ * 1) Subtree: "{param}"  -> Parent: "{myParam}" -> Value: "Hello"
+ *    Classical remapping from one port to another, but you need to use the syntax
+ *    {myParam} to say that you are remapping the another port.
+ *
+ * 2) Subtree: "{param}" -> Value: "World"
+ *    syntax without {}, in this case param directly point to the string "World".
+ *
+ * 3) Subtree: "{param}" -> Parent: "{parent}"
+ *    Setting to true (or 1) the attribute "__autoremap", we are automatically remapping
+ *    each port. Usefull to avoid some boilerplate.
+
+ */
+class SubtreePlusNode : public DecoratorNode
+{
+public:
+  SubtreePlusNode(const std::string& name);
+
+  virtual ~SubtreePlusNode() override = default;
+
+private:
+  virtual BT::NodeStatus tick() override;
+
+  static PortsList providedPorts()
+  {
+      return { InputPort<bool>("__autoremap", false,
+                               "If true, all the ports with the same name will be remapped") };
+  }
+
+  virtual NodeType type() const override final
+  {
+    return NodeType::SUBTREE;
+  }
+};
+
+
+
+}
+
+#endif   // DECORATOR_SUBTREE_NODE_H
diff --git a/include/behaviortree_cpp_v3/decorators/timeout_node.h b/include/behaviortree_cpp_v3/decorators/timeout_node.h
new file mode 100644 (file)
index 0000000..ac34579
--- /dev/null
@@ -0,0 +1,124 @@
+#ifndef DECORATOR_TIMEOUT_NODE_H
+#define DECORATOR_TIMEOUT_NODE_H
+
+#include "behaviortree_cpp_v3/decorator_node.h"
+#include <atomic>
+#include "timer_queue.h"
+
+namespace BT
+{
+/**
+ * @brief The TimeoutNode will halt() a running child if
+ * the latter has been RUNNING for more than a give time.
+ * The timeout is in milliseconds and it is passed using the port "msec".
+ *
+ * If timeout is reached it returns FAILURE.
+ *
+ * Example:
+ *
+ * <Timeout msec="5000">
+ *    <KeepYourBreath/>
+ * </Timeout>
+ */
+template <typename _Clock = std::chrono::steady_clock, typename _Duration = std::chrono::steady_clock::duration>
+class TimeoutNode : public DecoratorNode
+{
+  public:
+    TimeoutNode(const std::string& name, unsigned milliseconds)
+    : DecoratorNode(name, {} ),
+      child_halted_(false),
+      timer_id_(0),
+      msec_(milliseconds),
+      read_parameter_from_ports_(false),
+      timeout_started_(false)
+    {
+        setRegistrationID("Timeout");
+    }
+
+    TimeoutNode(const std::string& name, const NodeConfiguration& config)
+    : DecoratorNode(name, config),
+      child_halted_(false),
+      timer_id_(0),
+      msec_(0),
+      read_parameter_from_ports_(true),
+      timeout_started_(false)
+    {
+    }
+
+    ~TimeoutNode() override
+    {
+        timer_.cancelAll();
+    }
+
+    static PortsList providedPorts()
+    {
+        return { InputPort<unsigned>("msec", "After a certain amount of time, "
+                                             "halt() the child if it is still running.") };
+    }
+
+  private:
+    TimerQueue<_Clock, _Duration> timer_ ;
+
+    virtual BT::NodeStatus tick() override
+    {
+        if( read_parameter_from_ports_ )
+        {
+            if( !getInput("msec", msec_) )
+            {
+                throw RuntimeError("Missing parameter [msec] in TimeoutNode");
+            }
+        }
+
+        if ( !timeout_started_ )
+        {
+            timeout_started_ = true;
+            setStatus(NodeStatus::RUNNING);
+            child_halted_ = false;
+
+            if (msec_ > 0)
+            {
+                timer_id_ = timer_.add(std::chrono::milliseconds(msec_),
+                                       [this](bool aborted)
+                                       {
+                                           std::unique_lock<std::mutex> lk( timeout_mutex_ );
+                                           if (!aborted && child()->status() == NodeStatus::RUNNING)
+                                           {
+                                               child_halted_ = true;
+                                               haltChild();
+                                           }
+                                       });
+            }
+        }
+
+        std::unique_lock<std::mutex> lk( timeout_mutex_ );
+
+        if (child_halted_)
+        {
+            timeout_started_ = false;
+            return NodeStatus::FAILURE;
+        }
+        else
+        {
+            auto child_status = child()->executeTick();
+            if (child_status != NodeStatus::RUNNING)
+            {
+                timeout_started_ = false;
+                timeout_mutex_.unlock();
+                timer_.cancel(timer_id_);
+                timeout_mutex_.lock();
+            }
+            return child_status;
+        }
+    }
+
+    std::atomic<bool> child_halted_;
+    uint64_t timer_id_;
+
+    unsigned msec_;
+    bool read_parameter_from_ports_;
+    bool timeout_started_;
+    std::mutex timeout_mutex_;
+};
+}
+
+#endif   // DEADLINE_NODE_H
diff --git a/include/behaviortree_cpp_v3/decorators/timer_queue.h b/include/behaviortree_cpp_v3/decorators/timer_queue.h
new file mode 100644 (file)
index 0000000..2715dff
--- /dev/null
@@ -0,0 +1,263 @@
+#ifndef TIMERQUEUE_H
+#define TIMERQUEUE_H
+
+#include <mutex>
+#include <condition_variable>
+#include <thread>
+#include <queue>
+#include <chrono>
+#include <assert.h>
+
+namespace BT
+{
+// http://www.crazygaze.com/blog/2016/03/24/portable-c-timer-queue/
+
+namespace details
+{
+class Semaphore
+{
+  public:
+    Semaphore(unsigned int count = 0) : m_count(count)
+    {
+    }
+
+    void notify()
+    {
+        std::unique_lock<std::mutex> lock(m_mtx);
+        m_count++;
+        m_cv.notify_one();
+    }
+
+    void wait()
+    {
+        std::unique_lock<std::mutex> lock(m_mtx);
+        m_cv.wait(lock, [this]() { return m_count > 0; });
+        m_count--;
+    }
+
+    template <class Clock, class Duration>
+    bool waitUntil(const std::chrono::time_point<Clock, Duration>& point)
+    {
+        std::unique_lock<std::mutex> lock(m_mtx);
+        if (!m_cv.wait_until(lock, point, [this]() { return m_count > 0; }))
+            return false;
+        m_count--;
+        return true;
+    }
+
+  private:
+    std::mutex m_mtx;
+    std::condition_variable m_cv;
+    unsigned int m_count;
+};
+}
+
+// Timer Queue
+//
+// Allows execution of handlers at a specified time in the future
+// Guarantees:
+//  - All handlers are executed ONCE, even if canceled (aborted parameter will
+//be set to true)
+//      - If TimerQueue is destroyed, it will cancel all handlers.
+//  - Handlers are ALWAYS executed in the Timer Queue worker thread.
+//  - Handlers execution order is NOT guaranteed
+//
+template <typename _Clock = std::chrono::steady_clock, typename _Duration = std::chrono::steady_clock::duration>
+class TimerQueue
+{
+  public:
+    TimerQueue()
+    {
+        m_th = std::thread([this] { run(); });
+    }
+
+    ~TimerQueue()
+    {
+        cancelAll();
+        // Abusing the timer queue to trigger the shutdown.
+        add(std::chrono::milliseconds(0), [this](bool) { m_finish = true; });
+        m_th.join();
+    }
+
+    //! Adds a new timer
+    // \return
+    //  Returns the ID of the new timer. You can use this ID to cancel the
+    // timer
+    uint64_t add(std::chrono::milliseconds milliseconds, std::function<void(bool)> handler)
+    {
+        WorkItem item;
+        item.end = _Clock::now() + milliseconds;
+        item.handler = std::move(handler);
+
+        std::unique_lock<std::mutex> lk(m_mtx);
+        uint64_t id = ++m_idcounter;
+        item.id = id;
+        m_items.push(std::move(item));
+        lk.unlock();
+
+        // Something changed, so wake up timer thread
+        m_checkWork.notify();
+        return id;
+    }
+
+    //! Cancels the specified timer
+    // \return
+    //  1 if the timer was cancelled.
+    //  0 if you were too late to cancel (or the timer ID was never valid to
+    // start with)
+    size_t cancel(uint64_t id)
+    {
+        // Instead of removing the item from the container (thus breaking the
+        // heap integrity), we set the item as having no handler, and put
+        // that handler on a new item at the top for immediate execution
+        // The timer thread will then ignore the original item, since it has no
+        // handler.
+        std::unique_lock<std::mutex> lk(m_mtx);
+        for (auto&& item : m_items.getContainer())
+        {
+            if (item.id == id && item.handler)
+            {
+                WorkItem newItem;
+                // Zero time, so it stays at the top for immediate execution
+                newItem.end = std::chrono::time_point<_Clock, _Duration>();
+                newItem.id = 0;   // Means it is a canceled item
+                // Move the handler from item to newitem.
+                // Also, we need to manually set the handler to nullptr, since
+                // the standard does not guarantee moving an std::function will
+                // empty it. Some STL implementation will empty it, others will
+                // not.
+                newItem.handler = std::move(item.handler);
+                item.handler = nullptr;
+                m_items.push(std::move(newItem));
+
+                lk.unlock();
+                // Something changed, so wake up timer thread
+                m_checkWork.notify();
+                return 1;
+            }
+        }
+        return 0;
+    }
+
+    //! Cancels all timers
+    // \return
+    //  The number of timers cancelled
+    size_t cancelAll()
+    {
+        // Setting all "end" to 0 (for immediate execution) is ok,
+        // since it maintains the heap integrity
+        std::unique_lock<std::mutex> lk(m_mtx);
+        for (auto&& item : m_items.getContainer())
+        {
+            if (item.id)
+            {
+                item.end = std::chrono::time_point<_Clock, _Duration>();
+                item.id = 0;
+            }
+        }
+        auto ret = m_items.size();
+
+        lk.unlock();
+        m_checkWork.notify();
+        return ret;
+    }
+
+  private:
+    TimerQueue(const TimerQueue&) = delete;
+    TimerQueue& operator=(const TimerQueue&) = delete;
+
+    void run()
+    {
+        while (!m_finish)
+        {
+            auto end = calcWaitTime();
+            if (end.first)
+            {
+                // Timers found, so wait until it expires (or something else
+                // changes)
+                m_checkWork.waitUntil(end.second);
+            }
+            else
+            {
+                // No timers exist, so wait forever until something changes
+                m_checkWork.wait();
+            }
+
+            // Check and execute as much work as possible, such as, all expired
+            // timers
+            checkWork();
+        }
+
+        // If we are shutting down, we should not have any items left,
+        // since the shutdown cancels all items
+        assert(m_items.size() == 0);
+    }
+
+    std::pair<bool, std::chrono::time_point<_Clock, _Duration>> calcWaitTime()
+    {
+        std::lock_guard<std::mutex> lk(m_mtx);
+        while (m_items.size())
+        {
+            if (m_items.top().handler)
+            {
+                // Item present, so return the new wait time
+                return std::make_pair(true, m_items.top().end);
+            }
+            else
+            {
+                // Discard empty handlers (they were cancelled)
+                m_items.pop();
+            }
+        }
+
+        // No items found, so return no wait time (causes the thread to wait
+        // indefinitely)
+        return std::make_pair(false, std::chrono::time_point<_Clock, _Duration>());
+    }
+
+    void checkWork()
+    {
+        std::unique_lock<std::mutex> lk(m_mtx);
+        while (m_items.size() && m_items.top().end <= _Clock::now())
+        {
+            WorkItem item(std::move(m_items.top()));
+            m_items.pop();
+
+            lk.unlock();
+            if (item.handler)
+                item.handler(item.id == 0);
+            lk.lock();
+        }
+    }
+
+    details::Semaphore m_checkWork;
+    std::thread m_th;
+    bool m_finish = false;
+    uint64_t m_idcounter = 0;
+
+    struct WorkItem
+    {
+        std::chrono::time_point<_Clock, _Duration> end;
+        uint64_t id;   // id==0 means it was cancelled
+        std::function<void(bool)> handler;
+        bool operator>(const WorkItem& other) const
+        {
+            return end > other.end;
+        }
+    };
+
+    std::mutex m_mtx;
+    // Inheriting from priority_queue, so we can access the internal container
+    class Queue
+      : public std::priority_queue<WorkItem, std::vector<WorkItem>, std::greater<WorkItem>>
+    {
+      public:
+        std::vector<WorkItem>& getContainer()
+        {
+            return this->c;
+        }
+    } m_items;
+};
+}
+
+#endif   // TIMERQUEUE_H
diff --git a/include/behaviortree_cpp_v3/exceptions.h b/include/behaviortree_cpp_v3/exceptions.h
new file mode 100644 (file)
index 0000000..b280810
--- /dev/null
@@ -0,0 +1,74 @@
+/* Copyright (C) 2015-2018 Michele Colledanchise -  All Rights Reserved
+ * Copyright (C) 2018-2020 Davide Faconti, Eurecat -  All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#ifndef BT_EXCEPTIONS_H
+#define BT_EXCEPTIONS_H
+
+#include <string>
+#include <stdexcept>
+#include "utils/strcat.hpp"
+
+namespace BT
+{
+class BehaviorTreeException : public std::exception
+{
+  public:
+
+    BehaviorTreeException(nonstd::string_view message):  message_(static_cast<std::string>(message))
+    {}
+
+    template <typename... SV>
+    BehaviorTreeException(const SV&... args): message_(StrCat (args...))
+    { }
+
+
+    const char* what() const noexcept
+    {
+        return message_.c_str();
+    }
+
+  private:
+    std::string message_;
+};
+
+// This errors are usually related to problems that "probably" require code refactoring
+// to be fixed.
+class LogicError: public BehaviorTreeException
+{
+  public:
+    LogicError(nonstd::string_view message):  BehaviorTreeException(message)
+    {}
+
+    template <typename... SV>
+    LogicError(const SV&... args): BehaviorTreeException(args...)
+    { }
+
+};
+
+// This errors are usually related to problems that are relted to data or conditions
+// that happen only at run-time
+class RuntimeError: public BehaviorTreeException
+{
+  public:
+    RuntimeError(nonstd::string_view message):  BehaviorTreeException(message)
+    {}
+
+    template <typename... SV>
+    RuntimeError(const SV&... args): BehaviorTreeException(args...)
+    { }
+};
+
+
+}
+
+#endif
diff --git a/include/behaviortree_cpp_v3/flatbuffers/BT_logger.fbs b/include/behaviortree_cpp_v3/flatbuffers/BT_logger.fbs
new file mode 100644 (file)
index 0000000..4a225b9
--- /dev/null
@@ -0,0 +1,86 @@
+namespace Serialization;
+
+enum NodeStatus : byte {
+    IDLE = 0,
+    RUNNING,
+    SUCCESS,
+    FAILURE
+}
+
+enum NodeType : byte {
+    UNDEFINED = 0,
+    ACTION,
+    CONDITION,
+    CONTROL,
+    DECORATOR,
+    SUBTREE
+}
+
+enum PortDirection : byte {
+    INPUT = 0,
+    OUTPUT,
+    INOUT
+}
+
+table PortModel
+{
+  port_name : string;
+  direction : PortDirection;
+  type_info : string;
+  description: string;
+}
+
+table PortConfig
+{
+  port_name : string;
+  remap : string;
+}
+
+table TreeNode
+{
+  uid           : uint16;
+  children_uid  : [uint16];
+  status        : NodeStatus;
+  instance_name : string   (required);
+  registration_name : string (required);
+  port_remaps  : [PortConfig];
+}
+
+table NodeModel
+{
+  registration_name : string (required);
+  type : NodeType;
+  ports : [PortModel];
+}
+
+
+table BehaviorTree 
+{
+  root_uid : uint16;
+  nodes    : [TreeNode];
+  node_models : [NodeModel];
+}
+
+struct Timestamp
+{
+  usec_since_epoch : uint64;
+}
+
+
+struct StatusChange 
+{
+  uid         : uint16;
+  prev_status : NodeStatus;
+  status      : NodeStatus;
+  timestamp   : Timestamp;
+}
+
+table StatusChangeLog
+{
+    behavior_tree  : BehaviorTree;
+    state_changes  : [StatusChange];
+}
+
+root_type StatusChangeLog;
+
+root_type BehaviorTree;
diff --git a/include/behaviortree_cpp_v3/flatbuffers/BT_logger_generated.h b/include/behaviortree_cpp_v3/flatbuffers/BT_logger_generated.h
new file mode 100644 (file)
index 0000000..70808c8
--- /dev/null
@@ -0,0 +1,714 @@
+// automatically generated by the FlatBuffers compiler, do not modify
+
+
+#ifndef FLATBUFFERS_GENERATED_BTLOGGER_SERIALIZATION_H_
+#define FLATBUFFERS_GENERATED_BTLOGGER_SERIALIZATION_H_
+
+#include "behaviortree_cpp_v3/flatbuffers/flatbuffers.h"
+
+namespace Serialization {
+
+struct PortModel;
+
+struct PortConfig;
+
+struct TreeNode;
+
+struct NodeModel;
+
+struct BehaviorTree;
+
+struct Timestamp;
+
+struct StatusChange;
+
+struct StatusChangeLog;
+
+enum class NodeStatus : int8_t {
+  IDLE = 0,
+  RUNNING = 1,
+  SUCCESS = 2,
+  FAILURE = 3,
+  MIN = IDLE,
+  MAX = FAILURE
+};
+
+inline const NodeStatus (&EnumValuesNodeStatus())[4] {
+  static const NodeStatus values[] = {
+    NodeStatus::IDLE,
+    NodeStatus::RUNNING,
+    NodeStatus::SUCCESS,
+    NodeStatus::FAILURE
+  };
+  return values;
+}
+
+inline const char * const *EnumNamesNodeStatus() {
+  static const char * const names[] = {
+    "IDLE",
+    "RUNNING",
+    "SUCCESS",
+    "FAILURE",
+    nullptr
+  };
+  return names;
+}
+
+inline const char *EnumNameNodeStatus(NodeStatus e) {
+  if (e < NodeStatus::IDLE || e > NodeStatus::FAILURE) return "";
+  const size_t index = static_cast<int>(e);
+  return EnumNamesNodeStatus()[index];
+}
+
+enum class NodeType : int8_t {
+  UNDEFINED = 0,
+  ACTION = 1,
+  CONDITION = 2,
+  CONTROL = 3,
+  DECORATOR = 4,
+  SUBTREE = 5,
+  MIN = UNDEFINED,
+  MAX = SUBTREE
+};
+
+inline const NodeType (&EnumValuesNodeType())[6] {
+  static const NodeType values[] = {
+    NodeType::UNDEFINED,
+    NodeType::ACTION,
+    NodeType::CONDITION,
+    NodeType::CONTROL,
+    NodeType::DECORATOR,
+    NodeType::SUBTREE
+  };
+  return values;
+}
+
+inline const char * const *EnumNamesNodeType() {
+  static const char * const names[] = {
+    "UNDEFINED",
+    "ACTION",
+    "CONDITION",
+    "CONTROL",
+    "DECORATOR",
+    "SUBTREE",
+    nullptr
+  };
+  return names;
+}
+
+inline const char *EnumNameNodeType(NodeType e) {
+  if (e < NodeType::UNDEFINED || e > NodeType::SUBTREE) return "";
+  const size_t index = static_cast<int>(e);
+  return EnumNamesNodeType()[index];
+}
+
+enum class PortDirection : int8_t {
+  INPUT = 0,
+  OUTPUT = 1,
+  INOUT = 2,
+  MIN = INPUT,
+  MAX = INOUT
+};
+
+inline const PortDirection (&EnumValuesPortDirection())[3] {
+  static const PortDirection values[] = {
+    PortDirection::INPUT,
+    PortDirection::OUTPUT,
+    PortDirection::INOUT
+  };
+  return values;
+}
+
+inline const char * const *EnumNamesPortDirection() {
+  static const char * const names[] = {
+    "INPUT",
+    "OUTPUT",
+    "INOUT",
+    nullptr
+  };
+  return names;
+}
+
+inline const char *EnumNamePortDirection(PortDirection e) {
+  if (e < PortDirection::INPUT || e > PortDirection::INOUT) return "";
+  const size_t index = static_cast<int>(e);
+  return EnumNamesPortDirection()[index];
+}
+
+FLATBUFFERS_MANUALLY_ALIGNED_STRUCT(8) Timestamp FLATBUFFERS_FINAL_CLASS {
+ private:
+  uint64_t usec_since_epoch_;
+
+ public:
+  Timestamp() {
+    memset(this, 0, sizeof(Timestamp));
+  }
+  Timestamp(uint64_t _usec_since_epoch)
+      : usec_since_epoch_(flatbuffers::EndianScalar(_usec_since_epoch)) {
+  }
+  uint64_t usec_since_epoch() const {
+    return flatbuffers::EndianScalar(usec_since_epoch_);
+  }
+};
+FLATBUFFERS_STRUCT_END(Timestamp, 8);
+
+FLATBUFFERS_MANUALLY_ALIGNED_STRUCT(8) StatusChange FLATBUFFERS_FINAL_CLASS {
+ private:
+  uint16_t uid_;
+  int8_t prev_status_;
+  int8_t status_;
+  int32_t padding0__;
+  Timestamp timestamp_;
+
+ public:
+  StatusChange() {
+    memset(this, 0, sizeof(StatusChange));
+  }
+  StatusChange(uint16_t _uid, NodeStatus _prev_status, NodeStatus _status, const Timestamp &_timestamp)
+      : uid_(flatbuffers::EndianScalar(_uid)),
+        prev_status_(flatbuffers::EndianScalar(static_cast<int8_t>(_prev_status))),
+        status_(flatbuffers::EndianScalar(static_cast<int8_t>(_status))),
+        padding0__(0),
+        timestamp_(_timestamp) {
+    (void)padding0__;
+  }
+  uint16_t uid() const {
+    return flatbuffers::EndianScalar(uid_);
+  }
+  NodeStatus prev_status() const {
+    return static_cast<NodeStatus>(flatbuffers::EndianScalar(prev_status_));
+  }
+  NodeStatus status() const {
+    return static_cast<NodeStatus>(flatbuffers::EndianScalar(status_));
+  }
+  const Timestamp &timestamp() const {
+    return timestamp_;
+  }
+};
+FLATBUFFERS_STRUCT_END(StatusChange, 16);
+
+struct PortModel FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table {
+  enum FlatBuffersVTableOffset FLATBUFFERS_VTABLE_UNDERLYING_TYPE {
+    VT_PORT_NAME = 4,
+    VT_DIRECTION = 6,
+    VT_TYPE_INFO = 8,
+    VT_DESCRIPTION = 10
+  };
+  const flatbuffers::String *port_name() const {
+    return GetPointer<const flatbuffers::String *>(VT_PORT_NAME);
+  }
+  PortDirection direction() const {
+    return static_cast<PortDirection>(GetField<int8_t>(VT_DIRECTION, 0));
+  }
+  const flatbuffers::String *type_info() const {
+    return GetPointer<const flatbuffers::String *>(VT_TYPE_INFO);
+  }
+  const flatbuffers::String *description() const {
+    return GetPointer<const flatbuffers::String *>(VT_DESCRIPTION);
+  }
+  bool Verify(flatbuffers::Verifier &verifier) const {
+    return VerifyTableStart(verifier) &&
+           VerifyOffset(verifier, VT_PORT_NAME) &&
+           verifier.VerifyString(port_name()) &&
+           VerifyField<int8_t>(verifier, VT_DIRECTION) &&
+           VerifyOffset(verifier, VT_TYPE_INFO) &&
+           verifier.VerifyString(type_info()) &&
+           VerifyOffset(verifier, VT_DESCRIPTION) &&
+           verifier.VerifyString(description()) &&
+           verifier.EndTable();
+  }
+};
+
+struct PortModelBuilder {
+  flatbuffers::FlatBufferBuilder &fbb_;
+  flatbuffers::uoffset_t start_;
+  void add_port_name(flatbuffers::Offset<flatbuffers::String> port_name) {
+    fbb_.AddOffset(PortModel::VT_PORT_NAME, port_name);
+  }
+  void add_direction(PortDirection direction) {
+    fbb_.AddElement<int8_t>(PortModel::VT_DIRECTION, static_cast<int8_t>(direction), 0);
+  }
+  void add_type_info(flatbuffers::Offset<flatbuffers::String> type_info) {
+    fbb_.AddOffset(PortModel::VT_TYPE_INFO, type_info);
+  }
+  void add_description(flatbuffers::Offset<flatbuffers::String> description) {
+    fbb_.AddOffset(PortModel::VT_DESCRIPTION, description);
+  }
+  explicit PortModelBuilder(flatbuffers::FlatBufferBuilder &_fbb)
+        : fbb_(_fbb) {
+    start_ = fbb_.StartTable();
+  }
+  PortModelBuilder &operator=(const PortModelBuilder &);
+  flatbuffers::Offset<PortModel> Finish() {
+    const auto end = fbb_.EndTable(start_);
+    auto o = flatbuffers::Offset<PortModel>(end);
+    return o;
+  }
+};
+
+inline flatbuffers::Offset<PortModel> CreatePortModel(
+    flatbuffers::FlatBufferBuilder &_fbb,
+    flatbuffers::Offset<flatbuffers::String> port_name = 0,
+    PortDirection direction = PortDirection::INPUT,
+    flatbuffers::Offset<flatbuffers::String> type_info = 0,
+    flatbuffers::Offset<flatbuffers::String> description = 0) {
+  PortModelBuilder builder_(_fbb);
+  builder_.add_description(description);
+  builder_.add_type_info(type_info);
+  builder_.add_port_name(port_name);
+  builder_.add_direction(direction);
+  return builder_.Finish();
+}
+
+inline flatbuffers::Offset<PortModel> CreatePortModelDirect(
+    flatbuffers::FlatBufferBuilder &_fbb,
+    const char *port_name = nullptr,
+    PortDirection direction = PortDirection::INPUT,
+    const char *type_info = nullptr,
+    const char *description = nullptr) {
+  auto port_name__ = port_name ? _fbb.CreateString(port_name) : 0;
+  auto type_info__ = type_info ? _fbb.CreateString(type_info) : 0;
+  auto description__ = description ? _fbb.CreateString(description) : 0;
+  return Serialization::CreatePortModel(
+      _fbb,
+      port_name__,
+      direction,
+      type_info__,
+      description__);
+}
+
+struct PortConfig FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table {
+  enum FlatBuffersVTableOffset FLATBUFFERS_VTABLE_UNDERLYING_TYPE {
+    VT_PORT_NAME = 4,
+    VT_REMAP = 6
+  };
+  const flatbuffers::String *port_name() const {
+    return GetPointer<const flatbuffers::String *>(VT_PORT_NAME);
+  }
+  const flatbuffers::String *remap() const {
+    return GetPointer<const flatbuffers::String *>(VT_REMAP);
+  }
+  bool Verify(flatbuffers::Verifier &verifier) const {
+    return VerifyTableStart(verifier) &&
+           VerifyOffset(verifier, VT_PORT_NAME) &&
+           verifier.VerifyString(port_name()) &&
+           VerifyOffset(verifier, VT_REMAP) &&
+           verifier.VerifyString(remap()) &&
+           verifier.EndTable();
+  }
+};
+
+struct PortConfigBuilder {
+  flatbuffers::FlatBufferBuilder &fbb_;
+  flatbuffers::uoffset_t start_;
+  void add_port_name(flatbuffers::Offset<flatbuffers::String> port_name) {
+    fbb_.AddOffset(PortConfig::VT_PORT_NAME, port_name);
+  }
+  void add_remap(flatbuffers::Offset<flatbuffers::String> remap) {
+    fbb_.AddOffset(PortConfig::VT_REMAP, remap);
+  }
+  explicit PortConfigBuilder(flatbuffers::FlatBufferBuilder &_fbb)
+        : fbb_(_fbb) {
+    start_ = fbb_.StartTable();
+  }
+  PortConfigBuilder &operator=(const PortConfigBuilder &);
+  flatbuffers::Offset<PortConfig> Finish() {
+    const auto end = fbb_.EndTable(start_);
+    auto o = flatbuffers::Offset<PortConfig>(end);
+    return o;
+  }
+};
+
+inline flatbuffers::Offset<PortConfig> CreatePortConfig(
+    flatbuffers::FlatBufferBuilder &_fbb,
+    flatbuffers::Offset<flatbuffers::String> port_name = 0,
+    flatbuffers::Offset<flatbuffers::String> remap = 0) {
+  PortConfigBuilder builder_(_fbb);
+  builder_.add_remap(remap);
+  builder_.add_port_name(port_name);
+  return builder_.Finish();
+}
+
+inline flatbuffers::Offset<PortConfig> CreatePortConfigDirect(
+    flatbuffers::FlatBufferBuilder &_fbb,
+    const char *port_name = nullptr,
+    const char *remap = nullptr) {
+  auto port_name__ = port_name ? _fbb.CreateString(port_name) : 0;
+  auto remap__ = remap ? _fbb.CreateString(remap) : 0;
+  return Serialization::CreatePortConfig(
+      _fbb,
+      port_name__,
+      remap__);
+}
+
+struct TreeNode FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table {
+  enum FlatBuffersVTableOffset FLATBUFFERS_VTABLE_UNDERLYING_TYPE {
+    VT_UID = 4,
+    VT_CHILDREN_UID = 6,
+    VT_STATUS = 8,
+    VT_INSTANCE_NAME = 10,
+    VT_REGISTRATION_NAME = 12,
+    VT_PORT_REMAPS = 14
+  };
+  uint16_t uid() const {
+    return GetField<uint16_t>(VT_UID, 0);
+  }
+  const flatbuffers::Vector<uint16_t> *children_uid() const {
+    return GetPointer<const flatbuffers::Vector<uint16_t> *>(VT_CHILDREN_UID);
+  }
+  NodeStatus status() const {
+    return static_cast<NodeStatus>(GetField<int8_t>(VT_STATUS, 0));
+  }
+  const flatbuffers::String *instance_name() const {
+    return GetPointer<const flatbuffers::String *>(VT_INSTANCE_NAME);
+  }
+  const flatbuffers::String *registration_name() const {
+    return GetPointer<const flatbuffers::String *>(VT_REGISTRATION_NAME);
+  }
+  const flatbuffers::Vector<flatbuffers::Offset<PortConfig>> *port_remaps() const {
+    return GetPointer<const flatbuffers::Vector<flatbuffers::Offset<PortConfig>> *>(VT_PORT_REMAPS);
+  }
+  bool Verify(flatbuffers::Verifier &verifier) const {
+    return VerifyTableStart(verifier) &&
+           VerifyField<uint16_t>(verifier, VT_UID) &&
+           VerifyOffset(verifier, VT_CHILDREN_UID) &&
+           verifier.VerifyVector(children_uid()) &&
+           VerifyField<int8_t>(verifier, VT_STATUS) &&
+           VerifyOffsetRequired(verifier, VT_INSTANCE_NAME) &&
+           verifier.VerifyString(instance_name()) &&
+           VerifyOffsetRequired(verifier, VT_REGISTRATION_NAME) &&
+           verifier.VerifyString(registration_name()) &&
+           VerifyOffset(verifier, VT_PORT_REMAPS) &&
+           verifier.VerifyVector(port_remaps()) &&
+           verifier.VerifyVectorOfTables(port_remaps()) &&
+           verifier.EndTable();
+  }
+};
+
+struct TreeNodeBuilder {
+  flatbuffers::FlatBufferBuilder &fbb_;
+  flatbuffers::uoffset_t start_;
+  void add_uid(uint16_t uid) {
+    fbb_.AddElement<uint16_t>(TreeNode::VT_UID, uid, 0);
+  }
+  void add_children_uid(flatbuffers::Offset<flatbuffers::Vector<uint16_t>> children_uid) {
+    fbb_.AddOffset(TreeNode::VT_CHILDREN_UID, children_uid);
+  }
+  void add_status(NodeStatus status) {
+    fbb_.AddElement<int8_t>(TreeNode::VT_STATUS, static_cast<int8_t>(status), 0);
+  }
+  void add_instance_name(flatbuffers::Offset<flatbuffers::String> instance_name) {
+    fbb_.AddOffset(TreeNode::VT_INSTANCE_NAME, instance_name);
+  }
+  void add_registration_name(flatbuffers::Offset<flatbuffers::String> registration_name) {
+    fbb_.AddOffset(TreeNode::VT_REGISTRATION_NAME, registration_name);
+  }
+  void add_port_remaps(flatbuffers::Offset<flatbuffers::Vector<flatbuffers::Offset<PortConfig>>> port_remaps) {
+    fbb_.AddOffset(TreeNode::VT_PORT_REMAPS, port_remaps);
+  }
+  explicit TreeNodeBuilder(flatbuffers::FlatBufferBuilder &_fbb)
+        : fbb_(_fbb) {
+    start_ = fbb_.StartTable();
+  }
+  TreeNodeBuilder &operator=(const TreeNodeBuilder &);
+  flatbuffers::Offset<TreeNode> Finish() {
+    const auto end = fbb_.EndTable(start_);
+    auto o = flatbuffers::Offset<TreeNode>(end);
+    fbb_.Required(o, TreeNode::VT_INSTANCE_NAME);
+    fbb_.Required(o, TreeNode::VT_REGISTRATION_NAME);
+    return o;
+  }
+};
+
+inline flatbuffers::Offset<TreeNode> CreateTreeNode(
+    flatbuffers::FlatBufferBuilder &_fbb,
+    uint16_t uid = 0,
+    flatbuffers::Offset<flatbuffers::Vector<uint16_t>> children_uid = 0,
+    NodeStatus status = NodeStatus::IDLE,
+    flatbuffers::Offset<flatbuffers::String> instance_name = 0,
+    flatbuffers::Offset<flatbuffers::String> registration_name = 0,
+    flatbuffers::Offset<flatbuffers::Vector<flatbuffers::Offset<PortConfig>>> port_remaps = 0) {
+  TreeNodeBuilder builder_(_fbb);
+  builder_.add_port_remaps(port_remaps);
+  builder_.add_registration_name(registration_name);
+  builder_.add_instance_name(instance_name);
+  builder_.add_children_uid(children_uid);
+  builder_.add_uid(uid);
+  builder_.add_status(status);
+  return builder_.Finish();
+}
+
+inline flatbuffers::Offset<TreeNode> CreateTreeNodeDirect(
+    flatbuffers::FlatBufferBuilder &_fbb,
+    uint16_t uid = 0,
+    const std::vector<uint16_t> *children_uid = nullptr,
+    NodeStatus status = NodeStatus::IDLE,
+    const char *instance_name = nullptr,
+    const char *registration_name = nullptr,
+    const std::vector<flatbuffers::Offset<PortConfig>> *port_remaps = nullptr) {
+  auto children_uid__ = children_uid ? _fbb.CreateVector<uint16_t>(*children_uid) : 0;
+  auto instance_name__ = instance_name ? _fbb.CreateString(instance_name) : 0;
+  auto registration_name__ = registration_name ? _fbb.CreateString(registration_name) : 0;
+  auto port_remaps__ = port_remaps ? _fbb.CreateVector<flatbuffers::Offset<PortConfig>>(*port_remaps) : 0;
+  return Serialization::CreateTreeNode(
+      _fbb,
+      uid,
+      children_uid__,
+      status,
+      instance_name__,
+      registration_name__,
+      port_remaps__);
+}
+
+struct NodeModel FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table {
+  enum FlatBuffersVTableOffset FLATBUFFERS_VTABLE_UNDERLYING_TYPE {
+    VT_REGISTRATION_NAME = 4,
+    VT_TYPE = 6,
+    VT_PORTS = 8
+  };
+  const flatbuffers::String *registration_name() const {
+    return GetPointer<const flatbuffers::String *>(VT_REGISTRATION_NAME);
+  }
+  NodeType type() const {
+    return static_cast<NodeType>(GetField<int8_t>(VT_TYPE, 0));
+  }
+  const flatbuffers::Vector<flatbuffers::Offset<PortModel>> *ports() const {
+    return GetPointer<const flatbuffers::Vector<flatbuffers::Offset<PortModel>> *>(VT_PORTS);
+  }
+  bool Verify(flatbuffers::Verifier &verifier) const {
+    return VerifyTableStart(verifier) &&
+           VerifyOffsetRequired(verifier, VT_REGISTRATION_NAME) &&
+           verifier.VerifyString(registration_name()) &&
+           VerifyField<int8_t>(verifier, VT_TYPE) &&
+           VerifyOffset(verifier, VT_PORTS) &&
+           verifier.VerifyVector(ports()) &&
+           verifier.VerifyVectorOfTables(ports()) &&
+           verifier.EndTable();
+  }
+};
+
+struct NodeModelBuilder {
+  flatbuffers::FlatBufferBuilder &fbb_;
+  flatbuffers::uoffset_t start_;
+  void add_registration_name(flatbuffers::Offset<flatbuffers::String> registration_name) {
+    fbb_.AddOffset(NodeModel::VT_REGISTRATION_NAME, registration_name);
+  }
+  void add_type(NodeType type) {
+    fbb_.AddElement<int8_t>(NodeModel::VT_TYPE, static_cast<int8_t>(type), 0);
+  }
+  void add_ports(flatbuffers::Offset<flatbuffers::Vector<flatbuffers::Offset<PortModel>>> ports) {
+    fbb_.AddOffset(NodeModel::VT_PORTS, ports);
+  }
+  explicit NodeModelBuilder(flatbuffers::FlatBufferBuilder &_fbb)
+        : fbb_(_fbb) {
+    start_ = fbb_.StartTable();
+  }
+  NodeModelBuilder &operator=(const NodeModelBuilder &);
+  flatbuffers::Offset<NodeModel> Finish() {
+    const auto end = fbb_.EndTable(start_);
+    auto o = flatbuffers::Offset<NodeModel>(end);
+    fbb_.Required(o, NodeModel::VT_REGISTRATION_NAME);
+    return o;
+  }
+};
+
+inline flatbuffers::Offset<NodeModel> CreateNodeModel(
+    flatbuffers::FlatBufferBuilder &_fbb,
+    flatbuffers::Offset<flatbuffers::String> registration_name = 0,
+    NodeType type = NodeType::UNDEFINED,
+    flatbuffers::Offset<flatbuffers::Vector<flatbuffers::Offset<PortModel>>> ports = 0) {
+  NodeModelBuilder builder_(_fbb);
+  builder_.add_ports(ports);
+  builder_.add_registration_name(registration_name);
+  builder_.add_type(type);
+  return builder_.Finish();
+}
+
+inline flatbuffers::Offset<NodeModel> CreateNodeModelDirect(
+    flatbuffers::FlatBufferBuilder &_fbb,
+    const char *registration_name = nullptr,
+    NodeType type = NodeType::UNDEFINED,
+    const std::vector<flatbuffers::Offset<PortModel>> *ports = nullptr) {
+  auto registration_name__ = registration_name ? _fbb.CreateString(registration_name) : 0;
+  auto ports__ = ports ? _fbb.CreateVector<flatbuffers::Offset<PortModel>>(*ports) : 0;
+  return Serialization::CreateNodeModel(
+      _fbb,
+      registration_name__,
+      type,
+      ports__);
+}
+
+struct BehaviorTree FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table {
+  enum FlatBuffersVTableOffset FLATBUFFERS_VTABLE_UNDERLYING_TYPE {
+    VT_ROOT_UID = 4,
+    VT_NODES = 6,
+    VT_NODE_MODELS = 8
+  };
+  uint16_t root_uid() const {
+    return GetField<uint16_t>(VT_ROOT_UID, 0);
+  }
+  const flatbuffers::Vector<flatbuffers::Offset<TreeNode>> *nodes() const {
+    return GetPointer<const flatbuffers::Vector<flatbuffers::Offset<TreeNode>> *>(VT_NODES);
+  }
+  const flatbuffers::Vector<flatbuffers::Offset<NodeModel>> *node_models() const {
+    return GetPointer<const flatbuffers::Vector<flatbuffers::Offset<NodeModel>> *>(VT_NODE_MODELS);
+  }
+  bool Verify(flatbuffers::Verifier &verifier) const {
+    return VerifyTableStart(verifier) &&
+           VerifyField<uint16_t>(verifier, VT_ROOT_UID) &&
+           VerifyOffset(verifier, VT_NODES) &&
+           verifier.VerifyVector(nodes()) &&
+           verifier.VerifyVectorOfTables(nodes()) &&
+           VerifyOffset(verifier, VT_NODE_MODELS) &&
+           verifier.VerifyVector(node_models()) &&
+           verifier.VerifyVectorOfTables(node_models()) &&
+           verifier.EndTable();
+  }
+};
+
+struct BehaviorTreeBuilder {
+  flatbuffers::FlatBufferBuilder &fbb_;
+  flatbuffers::uoffset_t start_;
+  void add_root_uid(uint16_t root_uid) {
+    fbb_.AddElement<uint16_t>(BehaviorTree::VT_ROOT_UID, root_uid, 0);
+  }
+  void add_nodes(flatbuffers::Offset<flatbuffers::Vector<flatbuffers::Offset<TreeNode>>> nodes) {
+    fbb_.AddOffset(BehaviorTree::VT_NODES, nodes);
+  }
+  void add_node_models(flatbuffers::Offset<flatbuffers::Vector<flatbuffers::Offset<NodeModel>>> node_models) {
+    fbb_.AddOffset(BehaviorTree::VT_NODE_MODELS, node_models);
+  }
+  explicit BehaviorTreeBuilder(flatbuffers::FlatBufferBuilder &_fbb)
+        : fbb_(_fbb) {
+    start_ = fbb_.StartTable();
+  }
+  BehaviorTreeBuilder &operator=(const BehaviorTreeBuilder &);
+  flatbuffers::Offset<BehaviorTree> Finish() {
+    const auto end = fbb_.EndTable(start_);
+    auto o = flatbuffers::Offset<BehaviorTree>(end);
+    return o;
+  }
+};
+
+inline flatbuffers::Offset<BehaviorTree> CreateBehaviorTree(
+    flatbuffers::FlatBufferBuilder &_fbb,
+    uint16_t root_uid = 0,
+    flatbuffers::Offset<flatbuffers::Vector<flatbuffers::Offset<TreeNode>>> nodes = 0,
+    flatbuffers::Offset<flatbuffers::Vector<flatbuffers::Offset<NodeModel>>> node_models = 0) {
+  BehaviorTreeBuilder builder_(_fbb);
+  builder_.add_node_models(node_models);
+  builder_.add_nodes(nodes);
+  builder_.add_root_uid(root_uid);
+  return builder_.Finish();
+}
+
+inline flatbuffers::Offset<BehaviorTree> CreateBehaviorTreeDirect(
+    flatbuffers::FlatBufferBuilder &_fbb,
+    uint16_t root_uid = 0,
+    const std::vector<flatbuffers::Offset<TreeNode>> *nodes = nullptr,
+    const std::vector<flatbuffers::Offset<NodeModel>> *node_models = nullptr) {
+  auto nodes__ = nodes ? _fbb.CreateVector<flatbuffers::Offset<TreeNode>>(*nodes) : 0;
+  auto node_models__ = node_models ? _fbb.CreateVector<flatbuffers::Offset<NodeModel>>(*node_models) : 0;
+  return Serialization::CreateBehaviorTree(
+      _fbb,
+      root_uid,
+      nodes__,
+      node_models__);
+}
+
+struct StatusChangeLog FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table {
+  enum FlatBuffersVTableOffset FLATBUFFERS_VTABLE_UNDERLYING_TYPE {
+    VT_BEHAVIOR_TREE = 4,
+    VT_STATE_CHANGES = 6
+  };
+  const BehaviorTree *behavior_tree() const {
+    return GetPointer<const BehaviorTree *>(VT_BEHAVIOR_TREE);
+  }
+  const flatbuffers::Vector<const StatusChange *> *state_changes() const {
+    return GetPointer<const flatbuffers::Vector<const StatusChange *> *>(VT_STATE_CHANGES);
+  }
+  bool Verify(flatbuffers::Verifier &verifier) const {
+    return VerifyTableStart(verifier) &&
+           VerifyOffset(verifier, VT_BEHAVIOR_TREE) &&
+           verifier.VerifyTable(behavior_tree()) &&
+           VerifyOffset(verifier, VT_STATE_CHANGES) &&
+           verifier.VerifyVector(state_changes()) &&
+           verifier.EndTable();
+  }
+};
+
+struct StatusChangeLogBuilder {
+  flatbuffers::FlatBufferBuilder &fbb_;
+  flatbuffers::uoffset_t start_;
+  void add_behavior_tree(flatbuffers::Offset<BehaviorTree> behavior_tree) {
+    fbb_.AddOffset(StatusChangeLog::VT_BEHAVIOR_TREE, behavior_tree);
+  }
+  void add_state_changes(flatbuffers::Offset<flatbuffers::Vector<const StatusChange *>> state_changes) {
+    fbb_.AddOffset(StatusChangeLog::VT_STATE_CHANGES, state_changes);
+  }
+  explicit StatusChangeLogBuilder(flatbuffers::FlatBufferBuilder &_fbb)
+        : fbb_(_fbb) {
+    start_ = fbb_.StartTable();
+  }
+  StatusChangeLogBuilder &operator=(const StatusChangeLogBuilder &);
+  flatbuffers::Offset<StatusChangeLog> Finish() {
+    const auto end = fbb_.EndTable(start_);
+    auto o = flatbuffers::Offset<StatusChangeLog>(end);
+    return o;
+  }
+};
+
+inline flatbuffers::Offset<StatusChangeLog> CreateStatusChangeLog(
+    flatbuffers::FlatBufferBuilder &_fbb,
+    flatbuffers::Offset<BehaviorTree> behavior_tree = 0,
+    flatbuffers::Offset<flatbuffers::Vector<const StatusChange *>> state_changes = 0) {
+  StatusChangeLogBuilder builder_(_fbb);
+  builder_.add_state_changes(state_changes);
+  builder_.add_behavior_tree(behavior_tree);
+  return builder_.Finish();
+}
+
+inline flatbuffers::Offset<StatusChangeLog> CreateStatusChangeLogDirect(
+    flatbuffers::FlatBufferBuilder &_fbb,
+    flatbuffers::Offset<BehaviorTree> behavior_tree = 0,
+    const std::vector<StatusChange> *state_changes = nullptr) {
+  auto state_changes__ = state_changes ? _fbb.CreateVectorOfStructs<StatusChange>(*state_changes) : 0;
+  return Serialization::CreateStatusChangeLog(
+      _fbb,
+      behavior_tree,
+      state_changes__);
+}
+
+inline const Serialization::BehaviorTree *GetBehaviorTree(const void *buf) {
+  return flatbuffers::GetRoot<Serialization::BehaviorTree>(buf);
+}
+
+inline const Serialization::BehaviorTree *GetSizePrefixedBehaviorTree(const void *buf) {
+  return flatbuffers::GetSizePrefixedRoot<Serialization::BehaviorTree>(buf);
+}
+
+inline bool VerifyBehaviorTreeBuffer(
+    flatbuffers::Verifier &verifier) {
+  return verifier.VerifyBuffer<Serialization::BehaviorTree>(nullptr);
+}
+
+inline bool VerifySizePrefixedBehaviorTreeBuffer(
+    flatbuffers::Verifier &verifier) {
+  return verifier.VerifySizePrefixedBuffer<Serialization::BehaviorTree>(nullptr);
+}
+
+inline void FinishBehaviorTreeBuffer(
+    flatbuffers::FlatBufferBuilder &fbb,
+    flatbuffers::Offset<Serialization::BehaviorTree> root) {
+  fbb.Finish(root);
+}
+
+inline void FinishSizePrefixedBehaviorTreeBuffer(
+    flatbuffers::FlatBufferBuilder &fbb,
+    flatbuffers::Offset<Serialization::BehaviorTree> root) {
+  fbb.FinishSizePrefixed(root);
+}
+
+}  // namespace Serialization
+
+#endif  // FLATBUFFERS_GENERATED_BTLOGGER_SERIALIZATION_H_
diff --git a/include/behaviortree_cpp_v3/flatbuffers/LICENSE.txt b/include/behaviortree_cpp_v3/flatbuffers/LICENSE.txt
new file mode 100644 (file)
index 0000000..a4c5efd
--- /dev/null
@@ -0,0 +1,202 @@
+
+                                 Apache License
+                           Version 2.0, January 2004
+                        http://www.apache.org/licenses/
+
+   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+   1. Definitions.
+
+      "License" shall mean the terms and conditions for use, reproduction,
+      and distribution as defined by Sections 1 through 9 of this document.
+
+      "Licensor" shall mean the copyright owner or entity authorized by
+      the copyright owner that is granting the License.
+
+      "Legal Entity" shall mean the union of the acting entity and all
+      other entities that control, are controlled by, or are under common
+      control with that entity. For the purposes of this definition,
+      "control" means (i) the power, direct or indirect, to cause the
+      direction or management of such entity, whether by contract or
+      otherwise, or (ii) ownership of fifty percent (50%) or more of the
+      outstanding shares, or (iii) beneficial ownership of such entity.
+
+      "You" (or "Your") shall mean an individual or Legal Entity
+      exercising permissions granted by this License.
+
+      "Source" form shall mean the preferred form for making modifications,
+      including but not limited to software source code, documentation
+      source, and configuration files.
+
+      "Object" form shall mean any form resulting from mechanical
+      transformation or translation of a Source form, including but
+      not limited to compiled object code, generated documentation,
+      and conversions to other media types.
+
+      "Work" shall mean the work of authorship, whether in Source or
+      Object form, made available under the License, as indicated by a
+      copyright notice that is included in or attached to the work
+      (an example is provided in the Appendix below).
+
+      "Derivative Works" shall mean any work, whether in Source or Object
+      form, that is based on (or derived from) the Work and for which the
+      editorial revisions, annotations, elaborations, or other modifications
+      represent, as a whole, an original work of authorship. For the purposes
+      of this License, Derivative Works shall not include works that remain
+      separable from, or merely link (or bind by name) to the interfaces of,
+      the Work and Derivative Works thereof.
+
+      "Contribution" shall mean any work of authorship, including
+      the original version of the Work and any modifications or additions
+      to that Work or Derivative Works thereof, that is intentionally
+      submitted to Licensor for inclusion in the Work by the copyright owner
+      or by an individual or Legal Entity authorized to submit on behalf of
+      the copyright owner. For the purposes of this definition, "submitted"
+      means any form of electronic, verbal, or written communication sent
+      to the Licensor or its representatives, including but not limited to
+      communication on electronic mailing lists, source code control systems,
+      and issue tracking systems that are managed by, or on behalf of, the
+      Licensor for the purpose of discussing and improving the Work, but
+      excluding communication that is conspicuously marked or otherwise
+      designated in writing by the copyright owner as "Not a Contribution."
+
+      "Contributor" shall mean Licensor and any individual or Legal Entity
+      on behalf of whom a Contribution has been received by Licensor and
+      subsequently incorporated within the Work.
+
+   2. Grant of Copyright License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      copyright license to reproduce, prepare Derivative Works of,
+      publicly display, publicly perform, sublicense, and distribute the
+      Work and such Derivative Works in Source or Object form.
+
+   3. Grant of Patent License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      (except as stated in this section) patent license to make, have made,
+      use, offer to sell, sell, import, and otherwise transfer the Work,
+      where such license applies only to those patent claims licensable
+      by such Contributor that are necessarily infringed by their
+      Contribution(s) alone or by combination of their Contribution(s)
+      with the Work to which such Contribution(s) was submitted. If You
+      institute patent litigation against any entity (including a
+      cross-claim or counterclaim in a lawsuit) alleging that the Work
+      or a Contribution incorporated within the Work constitutes direct
+      or contributory patent infringement, then any patent licenses
+      granted to You under this License for that Work shall terminate
+      as of the date such litigation is filed.
+
+   4. Redistribution. You may reproduce and distribute copies of the
+      Work or Derivative Works thereof in any medium, with or without
+      modifications, and in Source or Object form, provided that You
+      meet the following conditions:
+
+      (a) You must give any other recipients of the Work or
+          Derivative Works a copy of this License; and
+
+      (b) You must cause any modified files to carry prominent notices
+          stating that You changed the files; and
+
+      (c) You must retain, in the Source form of any Derivative Works
+          that You distribute, all copyright, patent, trademark, and
+          attribution notices from the Source form of the Work,
+          excluding those notices that do not pertain to any part of
+          the Derivative Works; and
+
+      (d) If the Work includes a "NOTICE" text file as part of its
+          distribution, then any Derivative Works that You distribute must
+          include a readable copy of the attribution notices contained
+          within such NOTICE file, excluding those notices that do not
+          pertain to any part of the Derivative Works, in at least one
+          of the following places: within a NOTICE text file distributed
+          as part of the Derivative Works; within the Source form or
+          documentation, if provided along with the Derivative Works; or,
+          within a display generated by the Derivative Works, if and
+          wherever such third-party notices normally appear. The contents
+          of the NOTICE file are for informational purposes only and
+          do not modify the License. You may add Your own attribution
+          notices within Derivative Works that You distribute, alongside
+          or as an addendum to the NOTICE text from the Work, provided
+          that such additional attribution notices cannot be construed
+          as modifying the License.
+
+      You may add Your own copyright statement to Your modifications and
+      may provide additional or different license terms and conditions
+      for use, reproduction, or distribution of Your modifications, or
+      for any such Derivative Works as a whole, provided Your use,
+      reproduction, and distribution of the Work otherwise complies with
+      the conditions stated in this License.
+
+   5. Submission of Contributions. Unless You explicitly state otherwise,
+      any Contribution intentionally submitted for inclusion in the Work
+      by You to the Licensor shall be under the terms and conditions of
+      this License, without any additional terms or conditions.
+      Notwithstanding the above, nothing herein shall supersede or modify
+      the terms of any separate license agreement you may have executed
+      with Licensor regarding such Contributions.
+
+   6. Trademarks. This License does not grant permission to use the trade
+      names, trademarks, service marks, or product names of the Licensor,
+      except as required for reasonable and customary use in describing the
+      origin of the Work and reproducing the content of the NOTICE file.
+
+   7. Disclaimer of Warranty. Unless required by applicable law or
+      agreed to in writing, Licensor provides the Work (and each
+      Contributor provides its Contributions) on an "AS IS" BASIS,
+      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+      implied, including, without limitation, any warranties or conditions
+      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+      PARTICULAR PURPOSE. You are solely responsible for determining the
+      appropriateness of using or redistributing the Work and assume any
+      risks associated with Your exercise of permissions under this License.
+
+   8. Limitation of Liability. In no event and under no legal theory,
+      whether in tort (including negligence), contract, or otherwise,
+      unless required by applicable law (such as deliberate and grossly
+      negligent acts) or agreed to in writing, shall any Contributor be
+      liable to You for damages, including any direct, indirect, special,
+      incidental, or consequential damages of any character arising as a
+      result of this License or out of the use or inability to use the
+      Work (including but not limited to damages for loss of goodwill,
+      work stoppage, computer failure or malfunction, or any and all
+      other commercial damages or losses), even if such Contributor
+      has been advised of the possibility of such damages.
+
+   9. Accepting Warranty or Additional Liability. While redistributing
+      the Work or Derivative Works thereof, You may choose to offer,
+      and charge a fee for, acceptance of support, warranty, indemnity,
+      or other liability obligations and/or rights consistent with this
+      License. However, in accepting such obligations, You may act only
+      on Your own behalf and on Your sole responsibility, not on behalf
+      of any other Contributor, and only if You agree to indemnify,
+      defend, and hold each Contributor harmless for any liability
+      incurred by, or claims asserted against, such Contributor by reason
+      of your accepting any such warranty or additional liability.
+
+   END OF TERMS AND CONDITIONS
+
+   APPENDIX: How to apply the Apache License to your work.
+
+      To apply the Apache License to your work, attach the following
+      boilerplate notice, with the fields enclosed by brackets "[]"
+      replaced with your own identifying information. (Don't include
+      the brackets!)  The text should be enclosed in the appropriate
+      comment syntax for the file format. We also recommend that a
+      file or class name and description of purpose be included on the
+      same "printed page" as the copyright notice for easier
+      identification within third-party archives.
+
+   Copyright 2014 Google Inc.
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.
diff --git a/include/behaviortree_cpp_v3/flatbuffers/base.h b/include/behaviortree_cpp_v3/flatbuffers/base.h
new file mode 100644 (file)
index 0000000..1686dc6
--- /dev/null
@@ -0,0 +1,390 @@
+#ifndef FLATBUFFERS_BASE_H_
+#define FLATBUFFERS_BASE_H_
+
+// clang-format off
+
+// If activate should be declared and included first.
+#if defined(FLATBUFFERS_MEMORY_LEAK_TRACKING) && \
+    defined(_MSC_VER) && defined(_DEBUG)
+  // The _CRTDBG_MAP_ALLOC inside <crtdbg.h> will replace
+  // calloc/free (etc) to its debug version using #define directives.
+  #define _CRTDBG_MAP_ALLOC
+  #include <stdlib.h>
+  #include <crtdbg.h>
+  // Replace operator new by trace-enabled version.
+  #define DEBUG_NEW new(_NORMAL_BLOCK, __FILE__, __LINE__)
+  #define new DEBUG_NEW
+#endif
+
+#if !defined(FLATBUFFERS_ASSERT)
+#include <assert.h>
+#define FLATBUFFERS_ASSERT assert
+#elif defined(FLATBUFFERS_ASSERT_INCLUDE)
+// Include file with forward declaration
+#include FLATBUFFERS_ASSERT_INCLUDE
+#endif
+
+#ifndef ARDUINO
+#include <cstdint>
+#endif
+
+#include <cstddef>
+#include <cstdlib>
+#include <cstring>
+
+#if defined(ARDUINO) && !defined(ARDUINOSTL_M_H)
+  #include <utility.h>
+#else
+  #include <utility>
+#endif
+
+#include <string>
+#include <type_traits>
+#include <vector>
+#include <set>
+#include <algorithm>
+#include <iterator>
+#include <memory>
+
+#ifdef _STLPORT_VERSION
+  #define FLATBUFFERS_CPP98_STL
+#endif
+#ifndef FLATBUFFERS_CPP98_STL
+  #include <functional>
+#endif
+
+#include "behaviortree_cpp_v3/flatbuffers/stl_emulation.h"
+
+#if defined(__ICCARM__)
+#include <intrinsics.h>
+#endif
+
+// Note the __clang__ check is needed, because clang presents itself
+// as an older GNUC compiler (4.2).
+// Clang 3.3 and later implement all of the ISO C++ 2011 standard.
+// Clang 3.4 and later implement all of the ISO C++ 2014 standard.
+// http://clang.llvm.org/cxx_status.html
+
+// Note the MSVC value '__cplusplus' may be incorrect:
+// The '__cplusplus' predefined macro in the MSVC stuck at the value 199711L,
+// indicating (erroneously!) that the compiler conformed to the C++98 Standard.
+// This value should be correct starting from MSVC2017-15.7-Preview-3.
+// The '__cplusplus' will be valid only if MSVC2017-15.7-P3 and the `/Zc:__cplusplus` switch is set.
+// Workaround (for details see MSDN):
+// Use the _MSC_VER and _MSVC_LANG definition instead of the __cplusplus  for compatibility.
+// The _MSVC_LANG macro reports the Standard version regardless of the '/Zc:__cplusplus' switch.
+
+#if defined(__GNUC__) && !defined(__clang__)
+  #define FLATBUFFERS_GCC (__GNUC__ * 10000 + __GNUC_MINOR__ * 100 + __GNUC_PATCHLEVEL__)
+#else
+  #define FLATBUFFERS_GCC 0
+#endif
+
+#if defined(__clang__)
+  #define FLATBUFFERS_CLANG (__clang_major__ * 10000 + __clang_minor__ * 100 + __clang_patchlevel__)
+#else
+  #define FLATBUFFERS_CLANG 0
+#endif
+
+/// @cond FLATBUFFERS_INTERNAL
+#if __cplusplus <= 199711L && \
+    (!defined(_MSC_VER) || _MSC_VER < 1600) && \
+    (!defined(__GNUC__) || \
+      (__GNUC__ * 10000 + __GNUC_MINOR__ * 100 + __GNUC_PATCHLEVEL__ < 40400))
+  #error A C++11 compatible compiler with support for the auto typing is \
+         required for FlatBuffers.
+  #error __cplusplus _MSC_VER __GNUC__  __GNUC_MINOR__  __GNUC_PATCHLEVEL__
+#endif
+
+#if !defined(__clang__) && \
+    defined(__GNUC__) && \
+    (__GNUC__ * 10000 + __GNUC_MINOR__ * 100 + __GNUC_PATCHLEVEL__ < 40600)
+  // Backwards compatibility for g++ 4.4, and 4.5 which don't have the nullptr
+  // and constexpr keywords. Note the __clang__ check is needed, because clang
+  // presents itself as an older GNUC compiler.
+  #ifndef nullptr_t
+    const class nullptr_t {
+    public:
+      template<class T> inline operator T*() const { return 0; }
+    private:
+      void operator&() const;
+    } nullptr = {};
+  #endif
+  #ifndef constexpr
+    #define constexpr const
+  #endif
+#endif
+
+// The wire format uses a little endian encoding (since that's efficient for
+// the common platforms).
+#if defined(__s390x__)
+  #define FLATBUFFERS_LITTLEENDIAN 0
+#endif // __s390x__
+#if !defined(FLATBUFFERS_LITTLEENDIAN)
+  #if defined(__GNUC__) || defined(__clang__) || defined(__ICCARM__)
+    #if (defined(__BIG_ENDIAN__) || \
+         (defined(__BYTE_ORDER__) && __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__))
+      #define FLATBUFFERS_LITTLEENDIAN 0
+    #else
+      #define FLATBUFFERS_LITTLEENDIAN 1
+    #endif // __BIG_ENDIAN__
+  #elif defined(_MSC_VER)
+    #if defined(_M_PPC)
+      #define FLATBUFFERS_LITTLEENDIAN 0
+    #else
+      #define FLATBUFFERS_LITTLEENDIAN 1
+    #endif
+  #else
+    #error Unable to determine endianness, define FLATBUFFERS_LITTLEENDIAN.
+  #endif
+#endif // !defined(FLATBUFFERS_LITTLEENDIAN)
+
+#define FLATBUFFERS_VERSION_MAJOR 1
+#define FLATBUFFERS_VERSION_MINOR 11
+#define FLATBUFFERS_VERSION_REVISION 0
+#define FLATBUFFERS_STRING_EXPAND(X) #X
+#define FLATBUFFERS_STRING(X) FLATBUFFERS_STRING_EXPAND(X)
+namespace flatbuffers {
+  // Returns version as string  "MAJOR.MINOR.REVISION".
+  const char* FLATBUFFERS_VERSION();
+}
+
+#if (!defined(_MSC_VER) || _MSC_VER > 1600) && \
+    (!defined(__GNUC__) || (__GNUC__ * 100 + __GNUC_MINOR__ >= 407)) || \
+    defined(__clang__)
+  #define FLATBUFFERS_FINAL_CLASS final
+  #define FLATBUFFERS_OVERRIDE override
+  #define FLATBUFFERS_VTABLE_UNDERLYING_TYPE : flatbuffers::voffset_t
+#else
+  #define FLATBUFFERS_FINAL_CLASS
+  #define FLATBUFFERS_OVERRIDE
+  #define FLATBUFFERS_VTABLE_UNDERLYING_TYPE
+#endif
+
+#if (!defined(_MSC_VER) || _MSC_VER >= 1900) && \
+    (!defined(__GNUC__) || (__GNUC__ * 100 + __GNUC_MINOR__ >= 406)) || \
+    (defined(__cpp_constexpr) && __cpp_constexpr >= 200704)
+  #define FLATBUFFERS_CONSTEXPR constexpr
+#else
+  #define FLATBUFFERS_CONSTEXPR const
+#endif
+
+#if (defined(__cplusplus) && __cplusplus >= 201402L) || \
+    (defined(__cpp_constexpr) && __cpp_constexpr >= 201304)
+  #define FLATBUFFERS_CONSTEXPR_CPP14 FLATBUFFERS_CONSTEXPR
+#else
+  #define FLATBUFFERS_CONSTEXPR_CPP14
+#endif
+
+#if (defined(__GXX_EXPERIMENTAL_CXX0X__) && (__GNUC__ * 100 + __GNUC_MINOR__ >= 406)) || \
+    (defined(_MSC_FULL_VER) && (_MSC_FULL_VER >= 190023026)) || \
+    defined(__clang__)
+  #define FLATBUFFERS_NOEXCEPT noexcept
+#else
+  #define FLATBUFFERS_NOEXCEPT
+#endif
+
+// NOTE: the FLATBUFFERS_DELETE_FUNC macro may change the access mode to
+// private, so be sure to put it at the end or reset access mode explicitly.
+#if (!defined(_MSC_VER) || _MSC_FULL_VER >= 180020827) && \
+    (!defined(__GNUC__) || (__GNUC__ * 100 + __GNUC_MINOR__ >= 404)) || \
+    defined(__clang__)
+  #define FLATBUFFERS_DELETE_FUNC(func) func = delete;
+#else
+  #define FLATBUFFERS_DELETE_FUNC(func) private: func;
+#endif
+
+#ifndef FLATBUFFERS_HAS_STRING_VIEW
+  // Only provide flatbuffers::string_view if __has_include can be used
+  // to detect a header that provides an implementation
+  #if defined(__has_include)
+    // Check for std::string_view (in c++17)
+    #if __has_include(<string_view>) && (__cplusplus >= 201606 || (defined(_HAS_CXX17) && _HAS_CXX17))
+      #include <string_view>
+      namespace flatbuffers {
+        typedef std::string_view string_view;
+      }
+      #define FLATBUFFERS_HAS_STRING_VIEW 1
+    // Check for std::experimental::string_view (in c++14, compiler-dependent)
+    #elif __has_include(<experimental/string_view>) && (__cplusplus >= 201411)
+      #include <experimental/string_view>
+      namespace flatbuffers {
+        typedef std::experimental::string_view string_view;
+      }
+      #define FLATBUFFERS_HAS_STRING_VIEW 1
+    #endif
+  #endif // __has_include
+#endif // !FLATBUFFERS_HAS_STRING_VIEW
+
+#ifndef FLATBUFFERS_HAS_NEW_STRTOD
+  // Modern (C++11) strtod and strtof functions are available for use.
+  // 1) nan/inf strings as argument of strtod;
+  // 2) hex-float  as argument of  strtod/strtof.
+  #if (defined(_MSC_VER) && _MSC_VER >= 1900) || \
+      (defined(__GNUC__) && (__GNUC__ * 100 + __GNUC_MINOR__ >= 409)) || \
+      (defined(__clang__))
+    #define FLATBUFFERS_HAS_NEW_STRTOD 1
+  #endif
+#endif // !FLATBUFFERS_HAS_NEW_STRTOD
+
+#ifndef FLATBUFFERS_LOCALE_INDEPENDENT
+  // Enable locale independent functions {strtof_l, strtod_l,strtoll_l, strtoull_l}.
+  // They are part of the POSIX-2008 but not part of the C/C++ standard.
+  // GCC/Clang have definition (_XOPEN_SOURCE>=700) if POSIX-2008.
+  #if ((defined(_MSC_VER) && _MSC_VER >= 1800)            || \
+       (defined(_XOPEN_SOURCE) && (_XOPEN_SOURCE>=700)))
+    #define FLATBUFFERS_LOCALE_INDEPENDENT 1
+  #else
+    #define FLATBUFFERS_LOCALE_INDEPENDENT 0
+  #endif
+#endif  // !FLATBUFFERS_LOCALE_INDEPENDENT
+
+// Suppress Undefined Behavior Sanitizer (recoverable only). Usage:
+// - __supress_ubsan__("undefined")
+// - __supress_ubsan__("signed-integer-overflow")
+#if defined(__clang__) && (__clang_major__ > 3 || (__clang_major__ == 3 && __clang_minor__ >=7))
+  #define __supress_ubsan__(type) __attribute__((no_sanitize(type)))
+#elif defined(__GNUC__) && (__GNUC__ * 100 + __GNUC_MINOR__ >= 409)
+  #define __supress_ubsan__(type) __attribute__((no_sanitize_undefined))
+#else
+  #define __supress_ubsan__(type)
+#endif
+
+// This is constexpr function used for checking compile-time constants.
+// Avoid `#pragma warning(disable: 4127) // C4127: expression is constant`.
+template<typename T> FLATBUFFERS_CONSTEXPR inline bool IsConstTrue(T t) {
+  return !!t;
+}
+
+// Enable C++ attribute [[]] if std:c++17 or higher.
+#if ((__cplusplus >= 201703L) \
+    || (defined(_MSVC_LANG) &&  (_MSVC_LANG >= 201703L)))
+  // All attributes unknown to an implementation are ignored without causing an error.
+  #define FLATBUFFERS_ATTRIBUTE(attr) [[attr]]
+
+  #define FLATBUFFERS_FALLTHROUGH() [[fallthrough]]
+#else
+  #define FLATBUFFERS_ATTRIBUTE(attr)
+
+  #if FLATBUFFERS_CLANG >= 30800
+    #define FLATBUFFERS_FALLTHROUGH() [[clang::fallthrough]]
+  #elif FLATBUFFERS_GCC >= 70300
+    #define FLATBUFFERS_FALLTHROUGH() [[gnu::fallthrough]]
+  #else
+    #define FLATBUFFERS_FALLTHROUGH()
+  #endif
+#endif
+
+/// @endcond
+
+/// @file
+namespace flatbuffers {
+
+/// @cond FLATBUFFERS_INTERNAL
+// Our default offset / size type, 32bit on purpose on 64bit systems.
+// Also, using a consistent offset type maintains compatibility of serialized
+// offset values between 32bit and 64bit systems.
+typedef uint32_t uoffset_t;
+
+// Signed offsets for references that can go in both directions.
+typedef int32_t soffset_t;
+
+// Offset/index used in v-tables, can be changed to uint8_t in
+// format forks to save a bit of space if desired.
+typedef uint16_t voffset_t;
+
+typedef uintmax_t largest_scalar_t;
+
+// In 32bits, this evaluates to 2GB - 1
+#define FLATBUFFERS_MAX_BUFFER_SIZE ((1ULL << (sizeof(::flatbuffers::soffset_t) * 8 - 1)) - 1)
+
+// We support aligning the contents of buffers up to this size.
+#define FLATBUFFERS_MAX_ALIGNMENT 16
+
+#if defined(_MSC_VER)
+  #pragma warning(push)
+  #pragma warning(disable: 4127) // C4127: conditional expression is constant
+#endif
+
+template<typename T> T EndianSwap(T t) {
+  #if defined(_MSC_VER)
+    #define FLATBUFFERS_BYTESWAP16 _byteswap_ushort
+    #define FLATBUFFERS_BYTESWAP32 _byteswap_ulong
+    #define FLATBUFFERS_BYTESWAP64 _byteswap_uint64
+  #elif defined(__ICCARM__)
+    #define FLATBUFFERS_BYTESWAP16 __REV16
+    #define FLATBUFFERS_BYTESWAP32 __REV
+    #define FLATBUFFERS_BYTESWAP64(x) \
+       ((__REV(static_cast<uint32_t>(x >> 32U))) | (static_cast<uint64_t>(__REV(static_cast<uint32_t>(x)))) << 32U)
+  #else
+    #if defined(__GNUC__) && __GNUC__ * 100 + __GNUC_MINOR__ < 408 && !defined(__clang__)
+      // __builtin_bswap16 was missing prior to GCC 4.8.
+      #define FLATBUFFERS_BYTESWAP16(x) \
+        static_cast<uint16_t>(__builtin_bswap32(static_cast<uint32_t>(x) << 16))
+    #else
+      #define FLATBUFFERS_BYTESWAP16 __builtin_bswap16
+    #endif
+    #define FLATBUFFERS_BYTESWAP32 __builtin_bswap32
+    #define FLATBUFFERS_BYTESWAP64 __builtin_bswap64
+  #endif
+  if (sizeof(T) == 1) {   // Compile-time if-then's.
+    return t;
+  } else if (sizeof(T) == 2) {
+    union { T t; uint16_t i; } u = { t };
+    u.i = FLATBUFFERS_BYTESWAP16(u.i);
+    return u.t;
+  } else if (sizeof(T) == 4) {
+    union { T t; uint32_t i; } u = { t };
+    u.i = FLATBUFFERS_BYTESWAP32(u.i);
+    return u.t;
+  } else if (sizeof(T) == 8) {
+    union { T t; uint64_t i; } u = { t };
+    u.i = FLATBUFFERS_BYTESWAP64(u.i);
+    return u.t;
+  } else {
+    FLATBUFFERS_ASSERT(0);
+    return t;
+  }
+}
+
+#if defined(_MSC_VER)
+  #pragma warning(pop)
+#endif
+
+
+template<typename T> T EndianScalar(T t) {
+  #if FLATBUFFERS_LITTLEENDIAN
+    return t;
+  #else
+    return EndianSwap(t);
+  #endif
+}
+
+template<typename T>
+// UBSAN: C++ aliasing type rules, see std::bit_cast<> for details.
+__supress_ubsan__("alignment")
+T ReadScalar(const void *p) {
+  return EndianScalar(*reinterpret_cast<const T *>(p));
+}
+
+template<typename T>
+// UBSAN: C++ aliasing type rules, see std::bit_cast<> for details.
+__supress_ubsan__("alignment")
+void WriteScalar(void *p, T t) {
+  *reinterpret_cast<T *>(p) = EndianScalar(t);
+}
+
+template<typename T> struct Offset;
+template<typename T> __supress_ubsan__("alignment") void WriteScalar(void *p, Offset<T> t) {
+  *reinterpret_cast<uoffset_t *>(p) = EndianScalar(t.o);
+}
+
+// Computes how many bytes you'd have to pad to be able to write an
+// "scalar_size" scalar if the buffer had grown to "buf_size" (downwards in
+// memory).
+inline size_t PaddingBytes(size_t buf_size, size_t scalar_size) {
+  return ((~buf_size) + 1) & (scalar_size - 1);
+}
+
+}  // namespace flatbuffers
+#endif  // FLATBUFFERS_BASE_H_
diff --git a/include/behaviortree_cpp_v3/flatbuffers/bt_flatbuffer_helper.h b/include/behaviortree_cpp_v3/flatbuffers/bt_flatbuffer_helper.h
new file mode 100644 (file)
index 0000000..1c2a353
--- /dev/null
@@ -0,0 +1,171 @@
+#ifndef BT_FLATBUFFER_HELPER_H
+#define BT_FLATBUFFER_HELPER_H
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+#include "BT_logger_generated.h"
+
+namespace BT
+{
+
+typedef std::array<uint8_t, 12> SerializedTransition;
+
+inline Serialization::NodeType convertToFlatbuffers(BT::NodeType type)
+{
+    switch (type)
+    {
+        case BT::NodeType::ACTION:
+            return Serialization::NodeType::ACTION;
+        case BT::NodeType::DECORATOR:
+            return Serialization::NodeType::DECORATOR;
+        case BT::NodeType::CONTROL:
+            return Serialization::NodeType::CONTROL;
+        case BT::NodeType::CONDITION:
+            return Serialization::NodeType::CONDITION;
+        case BT::NodeType::SUBTREE:
+            return Serialization::NodeType::SUBTREE;
+        case BT::NodeType::UNDEFINED:
+            return Serialization::NodeType::UNDEFINED;
+    }
+    return Serialization::NodeType::UNDEFINED;
+}
+
+inline Serialization::NodeStatus convertToFlatbuffers(BT::NodeStatus type)
+{
+    switch (type)
+    {
+        case BT::NodeStatus::IDLE:
+            return Serialization::NodeStatus::IDLE;
+        case BT::NodeStatus::SUCCESS:
+            return Serialization::NodeStatus::SUCCESS;
+        case BT::NodeStatus::RUNNING:
+            return Serialization::NodeStatus::RUNNING;
+        case BT::NodeStatus::FAILURE:
+            return Serialization::NodeStatus::FAILURE;
+    }
+    return Serialization::NodeStatus::IDLE;
+}
+
+inline Serialization::PortDirection convertToFlatbuffers(BT::PortDirection direction)
+{
+    switch (direction)
+    {
+        case BT::PortDirection::INPUT :
+            return Serialization::PortDirection::INPUT;
+        case BT::PortDirection::OUTPUT:
+            return Serialization::PortDirection::OUTPUT;
+        case BT::PortDirection::INOUT:
+            return Serialization::PortDirection::INOUT;
+    }
+    return Serialization::PortDirection::INOUT;
+}
+
+inline void CreateFlatbuffersBehaviorTree(flatbuffers::FlatBufferBuilder& builder,
+                                          const BT::Tree& tree)
+{
+    std::vector<flatbuffers::Offset<Serialization::TreeNode>> fb_nodes;
+
+    applyRecursiveVisitor(tree.rootNode(), [&](BT::TreeNode* node)
+    {
+        std::vector<uint16_t> children_uid;
+        if (auto control = dynamic_cast<BT::ControlNode*>(node))
+        {
+            children_uid.reserve(control->children().size());
+            for (const auto& child : control->children())
+            {
+                children_uid.push_back(child->UID());
+            }
+        }
+        else if (auto decorator = dynamic_cast<BT::DecoratorNode*>(node))
+        {
+            const auto& child = decorator->child();
+            children_uid.push_back(child->UID());
+        }
+
+        std::vector<flatbuffers::Offset<Serialization::PortConfig>> ports;
+        for (const auto& it : node->config().input_ports)
+        {
+            ports.push_back(Serialization::CreatePortConfigDirect(
+                                builder, it.first.c_str(), it.second.c_str()));
+        }
+        for (const auto& it : node->config().output_ports)
+        {
+            ports.push_back(Serialization::CreatePortConfigDirect(
+                                builder, it.first.c_str(), it.second.c_str()));
+        }
+
+        auto tn = Serialization::CreateTreeNode(
+                    builder,
+                    node->UID(),
+                    builder.CreateVector(children_uid),
+                    convertToFlatbuffers(node->status()),
+                    builder.CreateString(node->name().c_str()),
+                    builder.CreateString(node->registrationName().c_str()),
+                    builder.CreateVector(ports));
+
+        fb_nodes.push_back(tn);
+    });
+
+    std::vector<flatbuffers::Offset<Serialization::NodeModel>> node_models;
+
+    for (const auto& node_it: tree.manifests)
+    {
+        const auto& manifest = node_it.second;
+        std::vector<flatbuffers::Offset<Serialization::PortModel>> port_models;
+
+        for (const auto& port_it: manifest.ports)
+        {
+            const auto& port_name = port_it.first;
+            const auto& port = port_it.second;
+            auto port_model = Serialization::CreatePortModel(
+                        builder,
+                        builder.CreateString( port_name.c_str() ),
+                        convertToFlatbuffers( port.direction() ),
+                        builder.CreateString( demangle(port.type()).c_str() ),
+                        builder.CreateString( port.description().c_str() )
+                        );
+            port_models.push_back(port_model);
+        }
+
+        auto node_model = Serialization::CreateNodeModel(
+                    builder,
+                    builder.CreateString(manifest.registration_ID.c_str()),
+                    convertToFlatbuffers(manifest.type),
+                    builder.CreateVector(port_models)  );
+
+        node_models.push_back(node_model);
+    }
+
+    auto behavior_tree = Serialization::CreateBehaviorTree(builder, tree.rootNode()->UID(),
+                                                           builder.CreateVector(fb_nodes),
+                                                           builder.CreateVector(node_models));
+
+    builder.Finish(behavior_tree);
+}
+
+/** Serialize manually the informations about state transition
+ * No flatbuffer serialization here
+ */
+inline SerializedTransition SerializeTransition(uint16_t UID,
+                                                Duration timestamp,
+                                                NodeStatus prev_status,
+                                                NodeStatus status)
+{
+    using namespace std::chrono;
+    SerializedTransition buffer;
+    int64_t usec = duration_cast<microseconds>(timestamp).count();
+    int64_t t_sec = usec / 1000000;
+    int64_t t_usec = usec % 1000000;
+
+    flatbuffers::WriteScalar(&buffer[0], t_sec);
+    flatbuffers::WriteScalar(&buffer[4], t_usec);
+    flatbuffers::WriteScalar(&buffer[8], UID);
+
+    flatbuffers::WriteScalar(&buffer[10], static_cast<int8_t>(convertToFlatbuffers(prev_status)));
+    flatbuffers::WriteScalar(&buffer[11], static_cast<int8_t>(convertToFlatbuffers(status)));
+
+    return buffer;
+}
+
+}   // end namespace
+
+#endif   // BT_FLATBUFFER_HELPER_H
diff --git a/include/behaviortree_cpp_v3/flatbuffers/flatbuffers.h b/include/behaviortree_cpp_v3/flatbuffers/flatbuffers.h
new file mode 100644 (file)
index 0000000..876285d
--- /dev/null
@@ -0,0 +1,2747 @@
+/*
+ * Copyright 2014 Google Inc. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef FLATBUFFERS_H_
+#define FLATBUFFERS_H_
+
+#include "behaviortree_cpp_v3/flatbuffers/base.h"
+
+#if defined(FLATBUFFERS_NAN_DEFAULTS)
+#  include <cmath>
+#endif
+
+namespace flatbuffers {
+// Generic 'operator==' with conditional specialisations.
+// T e - new value of a scalar field.
+// T def - default of scalar (is known at compile-time).
+template<typename T> inline bool IsTheSameAs(T e, T def) { return e == def; }
+
+#if defined(FLATBUFFERS_NAN_DEFAULTS) && \
+    defined(FLATBUFFERS_HAS_NEW_STRTOD) && (FLATBUFFERS_HAS_NEW_STRTOD > 0)
+// Like `operator==(e, def)` with weak NaN if T=(float|double).
+template<typename T> inline bool IsFloatTheSameAs(T e, T def) {
+  return (e == def) || ((def != def) && (e != e));
+}
+template<> inline bool IsTheSameAs<float>(float e, float def) {
+  return IsFloatTheSameAs(e, def);
+}
+template<> inline bool IsTheSameAs<double>(double e, double def) {
+  return IsFloatTheSameAs(e, def);
+}
+#endif
+
+// Wrapper for uoffset_t to allow safe template specialization.
+// Value is allowed to be 0 to indicate a null object (see e.g. AddOffset).
+template<typename T> struct Offset {
+  uoffset_t o;
+  Offset() : o(0) {}
+  Offset(uoffset_t _o) : o(_o) {}
+  Offset<void> Union() const { return Offset<void>(o); }
+  bool IsNull() const { return !o; }
+};
+
+inline void EndianCheck() {
+  int endiantest = 1;
+  // If this fails, see FLATBUFFERS_LITTLEENDIAN above.
+  FLATBUFFERS_ASSERT(*reinterpret_cast<char *>(&endiantest) ==
+                     FLATBUFFERS_LITTLEENDIAN);
+  (void)endiantest;
+}
+
+template<typename T> FLATBUFFERS_CONSTEXPR size_t AlignOf() {
+  // clang-format off
+  #ifdef _MSC_VER
+    return __alignof(T);
+  #else
+    #ifndef alignof
+      return __alignof__(T);
+    #else
+      return alignof(T);
+    #endif
+  #endif
+  // clang-format on
+}
+
+// When we read serialized data from memory, in the case of most scalars,
+// we want to just read T, but in the case of Offset, we want to actually
+// perform the indirection and return a pointer.
+// The template specialization below does just that.
+// It is wrapped in a struct since function templates can't overload on the
+// return type like this.
+// The typedef is for the convenience of callers of this function
+// (avoiding the need for a trailing return decltype)
+template<typename T> struct IndirectHelper {
+  typedef T return_type;
+  typedef T mutable_return_type;
+  static const size_t element_stride = sizeof(T);
+  static return_type Read(const uint8_t *p, uoffset_t i) {
+    return EndianScalar((reinterpret_cast<const T *>(p))[i]);
+  }
+};
+template<typename T> struct IndirectHelper<Offset<T>> {
+  typedef const T *return_type;
+  typedef T *mutable_return_type;
+  static const size_t element_stride = sizeof(uoffset_t);
+  static return_type Read(const uint8_t *p, uoffset_t i) {
+    p += i * sizeof(uoffset_t);
+    return reinterpret_cast<return_type>(p + ReadScalar<uoffset_t>(p));
+  }
+};
+template<typename T> struct IndirectHelper<const T *> {
+  typedef const T *return_type;
+  typedef T *mutable_return_type;
+  static const size_t element_stride = sizeof(T);
+  static return_type Read(const uint8_t *p, uoffset_t i) {
+    return reinterpret_cast<const T *>(p + i * sizeof(T));
+  }
+};
+
+// An STL compatible iterator implementation for Vector below, effectively
+// calling Get() for every element.
+template<typename T, typename IT> struct VectorIterator {
+  typedef std::random_access_iterator_tag iterator_category;
+  typedef IT value_type;
+  typedef ptrdiff_t difference_type;
+  typedef IT *pointer;
+  typedef IT &reference;
+
+  VectorIterator(const uint8_t *data, uoffset_t i)
+      : data_(data + IndirectHelper<T>::element_stride * i) {}
+  VectorIterator(const VectorIterator &other) : data_(other.data_) {}
+  VectorIterator() : data_(nullptr) {}
+
+  VectorIterator &operator=(const VectorIterator &other) {
+    data_ = other.data_;
+    return *this;
+  }
+
+  // clang-format off
+  #if !defined(FLATBUFFERS_CPP98_STL)
+  VectorIterator &operator=(VectorIterator &&other) {
+    data_ = other.data_;
+    return *this;
+  }
+  #endif  // !defined(FLATBUFFERS_CPP98_STL)
+  // clang-format on
+
+  bool operator==(const VectorIterator &other) const {
+    return data_ == other.data_;
+  }
+
+  bool operator<(const VectorIterator &other) const {
+    return data_ < other.data_;
+  }
+
+  bool operator!=(const VectorIterator &other) const {
+    return data_ != other.data_;
+  }
+
+  difference_type operator-(const VectorIterator &other) const {
+    return (data_ - other.data_) / IndirectHelper<T>::element_stride;
+  }
+
+  IT operator*() const { return IndirectHelper<T>::Read(data_, 0); }
+
+  IT operator->() const { return IndirectHelper<T>::Read(data_, 0); }
+
+  VectorIterator &operator++() {
+    data_ += IndirectHelper<T>::element_stride;
+    return *this;
+  }
+
+  VectorIterator operator++(int) {
+    VectorIterator temp(data_, 0);
+    data_ += IndirectHelper<T>::element_stride;
+    return temp;
+  }
+
+  VectorIterator operator+(const uoffset_t &offset) const {
+    return VectorIterator(data_ + offset * IndirectHelper<T>::element_stride,
+                          0);
+  }
+
+  VectorIterator &operator+=(const uoffset_t &offset) {
+    data_ += offset * IndirectHelper<T>::element_stride;
+    return *this;
+  }
+
+  VectorIterator &operator--() {
+    data_ -= IndirectHelper<T>::element_stride;
+    return *this;
+  }
+
+  VectorIterator operator--(int) {
+    VectorIterator temp(data_, 0);
+    data_ -= IndirectHelper<T>::element_stride;
+    return temp;
+  }
+
+  VectorIterator operator-(const uoffset_t &offset) const {
+    return VectorIterator(data_ - offset * IndirectHelper<T>::element_stride,
+                          0);
+  }
+
+  VectorIterator &operator-=(const uoffset_t &offset) {
+    data_ -= offset * IndirectHelper<T>::element_stride;
+    return *this;
+  }
+
+ private:
+  const uint8_t *data_;
+};
+
+template<typename Iterator>
+struct VectorReverseIterator : public std::reverse_iterator<Iterator> {
+  explicit VectorReverseIterator(Iterator iter)
+      : std::reverse_iterator<Iterator>(iter) {}
+
+  typename Iterator::value_type operator*() const {
+    return *(std::reverse_iterator<Iterator>::current);
+  }
+
+  typename Iterator::value_type operator->() const {
+    return *(std::reverse_iterator<Iterator>::current);
+  }
+};
+
+struct String;
+
+// This is used as a helper type for accessing vectors.
+// Vector::data() assumes the vector elements start after the length field.
+template<typename T> class Vector {
+ public:
+  typedef VectorIterator<T, typename IndirectHelper<T>::mutable_return_type>
+      iterator;
+  typedef VectorIterator<T, typename IndirectHelper<T>::return_type>
+      const_iterator;
+  typedef VectorReverseIterator<iterator> reverse_iterator;
+  typedef VectorReverseIterator<const_iterator> const_reverse_iterator;
+
+  uoffset_t size() const { return EndianScalar(length_); }
+
+  // Deprecated: use size(). Here for backwards compatibility.
+  FLATBUFFERS_ATTRIBUTE(deprecated("use size() instead"))
+  uoffset_t Length() const { return size(); }
+
+  typedef typename IndirectHelper<T>::return_type return_type;
+  typedef typename IndirectHelper<T>::mutable_return_type mutable_return_type;
+
+  return_type Get(uoffset_t i) const {
+    FLATBUFFERS_ASSERT(i < size());
+    return IndirectHelper<T>::Read(Data(), i);
+  }
+
+  return_type operator[](uoffset_t i) const { return Get(i); }
+
+  // If this is a Vector of enums, T will be its storage type, not the enum
+  // type. This function makes it convenient to retrieve value with enum
+  // type E.
+  template<typename E> E GetEnum(uoffset_t i) const {
+    return static_cast<E>(Get(i));
+  }
+
+  // If this a vector of unions, this does the cast for you. There's no check
+  // to make sure this is the right type!
+  template<typename U> const U *GetAs(uoffset_t i) const {
+    return reinterpret_cast<const U *>(Get(i));
+  }
+
+  // If this a vector of unions, this does the cast for you. There's no check
+  // to make sure this is actually a string!
+  const String *GetAsString(uoffset_t i) const {
+    return reinterpret_cast<const String *>(Get(i));
+  }
+
+  const void *GetStructFromOffset(size_t o) const {
+    return reinterpret_cast<const void *>(Data() + o);
+  }
+
+  iterator begin() { return iterator(Data(), 0); }
+  const_iterator begin() const { return const_iterator(Data(), 0); }
+
+  iterator end() { return iterator(Data(), size()); }
+  const_iterator end() const { return const_iterator(Data(), size()); }
+
+  reverse_iterator rbegin() { return reverse_iterator(end() - 1); }
+  const_reverse_iterator rbegin() const {
+    return const_reverse_iterator(end() - 1);
+  }
+
+  reverse_iterator rend() { return reverse_iterator(begin() - 1); }
+  const_reverse_iterator rend() const {
+    return const_reverse_iterator(begin() - 1);
+  }
+
+  const_iterator cbegin() const { return begin(); }
+
+  const_iterator cend() const { return end(); }
+
+  const_reverse_iterator crbegin() const { return rbegin(); }
+
+  const_reverse_iterator crend() const { return rend(); }
+
+  // Change elements if you have a non-const pointer to this object.
+  // Scalars only. See reflection.h, and the documentation.
+  void Mutate(uoffset_t i, const T &val) {
+    FLATBUFFERS_ASSERT(i < size());
+    WriteScalar(data() + i, val);
+  }
+
+  // Change an element of a vector of tables (or strings).
+  // "val" points to the new table/string, as you can obtain from
+  // e.g. reflection::AddFlatBuffer().
+  void MutateOffset(uoffset_t i, const uint8_t *val) {
+    FLATBUFFERS_ASSERT(i < size());
+    static_assert(sizeof(T) == sizeof(uoffset_t), "Unrelated types");
+    WriteScalar(data() + i,
+                static_cast<uoffset_t>(val - (Data() + i * sizeof(uoffset_t))));
+  }
+
+  // Get a mutable pointer to tables/strings inside this vector.
+  mutable_return_type GetMutableObject(uoffset_t i) const {
+    FLATBUFFERS_ASSERT(i < size());
+    return const_cast<mutable_return_type>(IndirectHelper<T>::Read(Data(), i));
+  }
+
+  // The raw data in little endian format. Use with care.
+  const uint8_t *Data() const {
+    return reinterpret_cast<const uint8_t *>(&length_ + 1);
+  }
+
+  uint8_t *Data() { return reinterpret_cast<uint8_t *>(&length_ + 1); }
+
+  // Similarly, but typed, much like std::vector::data
+  const T *data() const { return reinterpret_cast<const T *>(Data()); }
+  T *data() { return reinterpret_cast<T *>(Data()); }
+
+  template<typename K> return_type LookupByKey(K key) const {
+    void *search_result = std::bsearch(
+        &key, Data(), size(), IndirectHelper<T>::element_stride, KeyCompare<K>);
+
+    if (!search_result) {
+      return nullptr;  // Key not found.
+    }
+
+    const uint8_t *element = reinterpret_cast<const uint8_t *>(search_result);
+
+    return IndirectHelper<T>::Read(element, 0);
+  }
+
+ protected:
+  // This class is only used to access pre-existing data. Don't ever
+  // try to construct these manually.
+  Vector();
+
+  uoffset_t length_;
+
+ private:
+  // This class is a pointer. Copying will therefore create an invalid object.
+  // Private and unimplemented copy constructor.
+  Vector(const Vector &);
+
+  template<typename K> static int KeyCompare(const void *ap, const void *bp) {
+    const K *key = reinterpret_cast<const K *>(ap);
+    const uint8_t *data = reinterpret_cast<const uint8_t *>(bp);
+    auto table = IndirectHelper<T>::Read(data, 0);
+
+    // std::bsearch compares with the operands transposed, so we negate the
+    // result here.
+    return -table->KeyCompareWithValue(*key);
+  }
+};
+
+// Represent a vector much like the template above, but in this case we
+// don't know what the element types are (used with reflection.h).
+class VectorOfAny {
+ public:
+  uoffset_t size() const { return EndianScalar(length_); }
+
+  const uint8_t *Data() const {
+    return reinterpret_cast<const uint8_t *>(&length_ + 1);
+  }
+  uint8_t *Data() { return reinterpret_cast<uint8_t *>(&length_ + 1); }
+
+ protected:
+  VectorOfAny();
+
+  uoffset_t length_;
+
+ private:
+  VectorOfAny(const VectorOfAny &);
+};
+
+#ifndef FLATBUFFERS_CPP98_STL
+template<typename T, typename U>
+Vector<Offset<T>> *VectorCast(Vector<Offset<U>> *ptr) {
+  static_assert(std::is_base_of<T, U>::value, "Unrelated types");
+  return reinterpret_cast<Vector<Offset<T>> *>(ptr);
+}
+
+template<typename T, typename U>
+const Vector<Offset<T>> *VectorCast(const Vector<Offset<U>> *ptr) {
+  static_assert(std::is_base_of<T, U>::value, "Unrelated types");
+  return reinterpret_cast<const Vector<Offset<T>> *>(ptr);
+}
+#endif
+
+// Convenient helper function to get the length of any vector, regardless
+// of whether it is null or not (the field is not set).
+template<typename T> static inline size_t VectorLength(const Vector<T> *v) {
+  return v ? v->size() : 0;
+}
+
+// This is used as a helper type for accessing arrays.
+template<typename T, uint16_t length> class Array {
+  typedef
+      typename flatbuffers::integral_constant<bool,
+                                              flatbuffers::is_scalar<T>::value>
+          scalar_tag;
+  typedef
+      typename flatbuffers::conditional<scalar_tag::value, T, const T *>::type
+          IndirectHelperType;
+
+ public:
+  typedef typename IndirectHelper<IndirectHelperType>::return_type return_type;
+  typedef VectorIterator<T, return_type> const_iterator;
+  typedef VectorReverseIterator<const_iterator> const_reverse_iterator;
+
+  FLATBUFFERS_CONSTEXPR uint16_t size() const { return length; }
+
+  return_type Get(uoffset_t i) const {
+    FLATBUFFERS_ASSERT(i < size());
+    return IndirectHelper<IndirectHelperType>::Read(Data(), i);
+  }
+
+  return_type operator[](uoffset_t i) const { return Get(i); }
+
+  const_iterator begin() const { return const_iterator(Data(), 0); }
+  const_iterator end() const { return const_iterator(Data(), size()); }
+
+  const_reverse_iterator rbegin() const {
+    return const_reverse_iterator(end());
+  }
+  const_reverse_iterator rend() const { return const_reverse_iterator(end()); }
+
+  const_iterator cbegin() const { return begin(); }
+  const_iterator cend() const { return end(); }
+
+  const_reverse_iterator crbegin() const { return rbegin(); }
+  const_reverse_iterator crend() const { return rend(); }
+
+  // Get a mutable pointer to elements inside this array.
+  // This method used to mutate arrays of structs followed by a @p Mutate
+  // operation. For primitive types use @p Mutate directly.
+  // @warning Assignments and reads to/from the dereferenced pointer are not
+  //  automatically converted to the correct endianness.
+  typename flatbuffers::conditional<scalar_tag::value, void, T *>::type
+  GetMutablePointer(uoffset_t i) const {
+    FLATBUFFERS_ASSERT(i < size());
+    return const_cast<T *>(&data()[i]);
+  }
+
+  // Change elements if you have a non-const pointer to this object.
+  void Mutate(uoffset_t i, const T &val) { MutateImpl(scalar_tag(), i, val); }
+
+  // The raw data in little endian format. Use with care.
+  const uint8_t *Data() const { return data_; }
+
+  uint8_t *Data() { return data_; }
+
+  // Similarly, but typed, much like std::vector::data
+  const T *data() const { return reinterpret_cast<const T *>(Data()); }
+  T *data() { return reinterpret_cast<T *>(Data()); }
+
+ protected:
+  void MutateImpl(flatbuffers::integral_constant<bool, true>, uoffset_t i,
+                  const T &val) {
+    FLATBUFFERS_ASSERT(i < size());
+    WriteScalar(data() + i, val);
+  }
+
+  void MutateImpl(flatbuffers::integral_constant<bool, false>, uoffset_t i,
+                  const T &val) {
+    *(GetMutablePointer(i)) = val;
+  }
+
+  // This class is only used to access pre-existing data. Don't ever
+  // try to construct these manually.
+  // 'constexpr' allows us to use 'size()' at compile time.
+  // @note Must not use 'FLATBUFFERS_CONSTEXPR' here, as const is not allowed on
+  //  a constructor.
+#if defined(__cpp_constexpr)
+  constexpr Array();
+#else
+  Array();
+#endif
+
+  uint8_t data_[length * sizeof(T)];
+
+ private:
+  // This class is a pointer. Copying will therefore create an invalid object.
+  // Private and unimplemented copy constructor.
+  Array(const Array &);
+  Array &operator=(const Array &);
+};
+
+// Specialization for Array[struct] with access using Offset<void> pointer.
+// This specialization used by idl_gen_text.cpp.
+template<typename T, uint16_t length> class Array<Offset<T>, length> {
+  static_assert(flatbuffers::is_same<T, void>::value, "unexpected type T");
+
+ public:
+  const uint8_t *Data() const { return data_; }
+
+  // Make idl_gen_text.cpp::PrintContainer happy.
+  const void *operator[](uoffset_t) const {
+    FLATBUFFERS_ASSERT(false);
+    return nullptr;
+  }
+
+ private:
+  // This class is only used to access pre-existing data.
+  Array();
+  Array(const Array &);
+  Array &operator=(const Array &);
+
+  uint8_t data_[1];
+};
+
+// Lexicographically compare two strings (possibly containing nulls), and
+// return true if the first is less than the second.
+static inline bool StringLessThan(const char *a_data, uoffset_t a_size,
+                                  const char *b_data, uoffset_t b_size) {
+  const auto cmp = memcmp(a_data, b_data, (std::min)(a_size, b_size));
+  return cmp == 0 ? a_size < b_size : cmp < 0;
+}
+
+struct String : public Vector<char> {
+  const char *c_str() const { return reinterpret_cast<const char *>(Data()); }
+  std::string str() const { return std::string(c_str(), size()); }
+
+  // clang-format off
+  #ifdef FLATBUFFERS_HAS_STRING_VIEW
+  flatbuffers::string_view string_view() const {
+    return flatbuffers::string_view(c_str(), size());
+  }
+  #endif // FLATBUFFERS_HAS_STRING_VIEW
+  // clang-format on
+
+  bool operator<(const String &o) const {
+    return StringLessThan(this->data(), this->size(), o.data(), o.size());
+  }
+};
+
+// Convenience function to get std::string from a String returning an empty
+// string on null pointer.
+static inline std::string GetString(const String *str) {
+  return str ? str->str() : "";
+}
+
+// Convenience function to get char* from a String returning an empty string on
+// null pointer.
+static inline const char *GetCstring(const String *str) {
+  return str ? str->c_str() : "";
+}
+
+// Allocator interface. This is flatbuffers-specific and meant only for
+// `vector_downward` usage.
+class Allocator {
+ public:
+  virtual ~Allocator() {}
+
+  // Allocate `size` bytes of memory.
+  virtual uint8_t *allocate(size_t size) = 0;
+
+  // Deallocate `size` bytes of memory at `p` allocated by this allocator.
+  virtual void deallocate(uint8_t *p, size_t size) = 0;
+
+  // Reallocate `new_size` bytes of memory, replacing the old region of size
+  // `old_size` at `p`. In contrast to a normal realloc, this grows downwards,
+  // and is intended specifcally for `vector_downward` use.
+  // `in_use_back` and `in_use_front` indicate how much of `old_size` is
+  // actually in use at each end, and needs to be copied.
+  virtual uint8_t *reallocate_downward(uint8_t *old_p, size_t old_size,
+                                       size_t new_size, size_t in_use_back,
+                                       size_t in_use_front) {
+    FLATBUFFERS_ASSERT(new_size > old_size);  // vector_downward only grows
+    uint8_t *new_p = allocate(new_size);
+    memcpy_downward(old_p, old_size, new_p, new_size, in_use_back,
+                    in_use_front);
+    deallocate(old_p, old_size);
+    return new_p;
+  }
+
+ protected:
+  // Called by `reallocate_downward` to copy memory from `old_p` of `old_size`
+  // to `new_p` of `new_size`. Only memory of size `in_use_front` and
+  // `in_use_back` will be copied from the front and back of the old memory
+  // allocation.
+  void memcpy_downward(uint8_t *old_p, size_t old_size, uint8_t *new_p,
+                       size_t new_size, size_t in_use_back,
+                       size_t in_use_front) {
+    memcpy(new_p + new_size - in_use_back, old_p + old_size - in_use_back,
+           in_use_back);
+    memcpy(new_p, old_p, in_use_front);
+  }
+};
+
+// DefaultAllocator uses new/delete to allocate memory regions
+class DefaultAllocator : public Allocator {
+ public:
+  uint8_t *allocate(size_t size) FLATBUFFERS_OVERRIDE {
+    return new uint8_t[size];
+  }
+
+  void deallocate(uint8_t *p, size_t) FLATBUFFERS_OVERRIDE { delete[] p; }
+
+  static void dealloc(void *p, size_t) { delete[] static_cast<uint8_t *>(p); }
+};
+
+// These functions allow for a null allocator to mean use the default allocator,
+// as used by DetachedBuffer and vector_downward below.
+// This is to avoid having a statically or dynamically allocated default
+// allocator, or having to move it between the classes that may own it.
+inline uint8_t *Allocate(Allocator *allocator, size_t size) {
+  return allocator ? allocator->allocate(size)
+                   : DefaultAllocator().allocate(size);
+}
+
+inline void Deallocate(Allocator *allocator, uint8_t *p, size_t size) {
+  if (allocator)
+    allocator->deallocate(p, size);
+  else
+    DefaultAllocator().deallocate(p, size);
+}
+
+inline uint8_t *ReallocateDownward(Allocator *allocator, uint8_t *old_p,
+                                   size_t old_size, size_t new_size,
+                                   size_t in_use_back, size_t in_use_front) {
+  return allocator ? allocator->reallocate_downward(old_p, old_size, new_size,
+                                                    in_use_back, in_use_front)
+                   : DefaultAllocator().reallocate_downward(
+                         old_p, old_size, new_size, in_use_back, in_use_front);
+}
+
+// DetachedBuffer is a finished flatbuffer memory region, detached from its
+// builder. The original memory region and allocator are also stored so that
+// the DetachedBuffer can manage the memory lifetime.
+class DetachedBuffer {
+ public:
+  DetachedBuffer()
+      : allocator_(nullptr),
+        own_allocator_(false),
+        buf_(nullptr),
+        reserved_(0),
+        cur_(nullptr),
+        size_(0) {}
+
+  DetachedBuffer(Allocator *allocator, bool own_allocator, uint8_t *buf,
+                 size_t reserved, uint8_t *cur, size_t sz)
+      : allocator_(allocator),
+        own_allocator_(own_allocator),
+        buf_(buf),
+        reserved_(reserved),
+        cur_(cur),
+        size_(sz) {}
+
+  // clang-format off
+  #if !defined(FLATBUFFERS_CPP98_STL)
+  // clang-format on
+  DetachedBuffer(DetachedBuffer &&other)
+      : allocator_(other.allocator_),
+        own_allocator_(other.own_allocator_),
+        buf_(other.buf_),
+        reserved_(other.reserved_),
+        cur_(other.cur_),
+        size_(other.size_) {
+    other.reset();
+  }
+  // clang-format off
+  #endif  // !defined(FLATBUFFERS_CPP98_STL)
+  // clang-format on
+
+  // clang-format off
+  #if !defined(FLATBUFFERS_CPP98_STL)
+  // clang-format on
+  DetachedBuffer &operator=(DetachedBuffer &&other) {
+    if (this == &other) return *this;
+
+    destroy();
+
+    allocator_ = other.allocator_;
+    own_allocator_ = other.own_allocator_;
+    buf_ = other.buf_;
+    reserved_ = other.reserved_;
+    cur_ = other.cur_;
+    size_ = other.size_;
+
+    other.reset();
+
+    return *this;
+  }
+  // clang-format off
+  #endif  // !defined(FLATBUFFERS_CPP98_STL)
+  // clang-format on
+
+  ~DetachedBuffer() { destroy(); }
+
+  const uint8_t *data() const { return cur_; }
+
+  uint8_t *data() { return cur_; }
+
+  size_t size() const { return size_; }
+
+  // clang-format off
+  #if 0  // disabled for now due to the ordering of classes in this header
+  template <class T>
+  bool Verify() const {
+    Verifier verifier(data(), size());
+    return verifier.Verify<T>(nullptr);
+  }
+
+  template <class T>
+  const T* GetRoot() const {
+    return flatbuffers::GetRoot<T>(data());
+  }
+
+  template <class T>
+  T* GetRoot() {
+    return flatbuffers::GetRoot<T>(data());
+  }
+  #endif
+  // clang-format on
+
+  // clang-format off
+  #if !defined(FLATBUFFERS_CPP98_STL)
+  // clang-format on
+  // These may change access mode, leave these at end of public section
+  FLATBUFFERS_DELETE_FUNC(DetachedBuffer(const DetachedBuffer &other))
+  FLATBUFFERS_DELETE_FUNC(
+      DetachedBuffer &operator=(const DetachedBuffer &other))
+  // clang-format off
+  #endif  // !defined(FLATBUFFERS_CPP98_STL)
+  // clang-format on
+
+ protected:
+  Allocator *allocator_;
+  bool own_allocator_;
+  uint8_t *buf_;
+  size_t reserved_;
+  uint8_t *cur_;
+  size_t size_;
+
+  inline void destroy() {
+    if (buf_) Deallocate(allocator_, buf_, reserved_);
+    if (own_allocator_ && allocator_) { delete allocator_; }
+    reset();
+  }
+
+  inline void reset() {
+    allocator_ = nullptr;
+    own_allocator_ = false;
+    buf_ = nullptr;
+    reserved_ = 0;
+    cur_ = nullptr;
+    size_ = 0;
+  }
+};
+
+// This is a minimal replication of std::vector<uint8_t> functionality,
+// except growing from higher to lower addresses. i.e push_back() inserts data
+// in the lowest address in the vector.
+// Since this vector leaves the lower part unused, we support a "scratch-pad"
+// that can be stored there for temporary data, to share the allocated space.
+// Essentially, this supports 2 std::vectors in a single buffer.
+class vector_downward {
+ public:
+  explicit vector_downward(size_t initial_size, Allocator *allocator,
+                           bool own_allocator, size_t buffer_minalign)
+      : allocator_(allocator),
+        own_allocator_(own_allocator),
+        initial_size_(initial_size),
+        buffer_minalign_(buffer_minalign),
+        reserved_(0),
+        buf_(nullptr),
+        cur_(nullptr),
+        scratch_(nullptr) {}
+
+  // clang-format off
+  #if !defined(FLATBUFFERS_CPP98_STL)
+  vector_downward(vector_downward &&other)
+  #else
+  vector_downward(vector_downward &other)
+  #endif  // defined(FLATBUFFERS_CPP98_STL)
+      // clang-format on
+      : allocator_(other.allocator_),
+        own_allocator_(other.own_allocator_),
+        initial_size_(other.initial_size_),
+        buffer_minalign_(other.buffer_minalign_),
+        reserved_(other.reserved_),
+        buf_(other.buf_),
+        cur_(other.cur_),
+        scratch_(other.scratch_) {
+    // No change in other.allocator_
+    // No change in other.initial_size_
+    // No change in other.buffer_minalign_
+    other.own_allocator_ = false;
+    other.reserved_ = 0;
+    other.buf_ = nullptr;
+    other.cur_ = nullptr;
+    other.scratch_ = nullptr;
+  }
+
+  // clang-format off
+  #if !defined(FLATBUFFERS_CPP98_STL)
+  // clang-format on
+  vector_downward &operator=(vector_downward &&other) {
+    // Move construct a temporary and swap idiom
+    vector_downward temp(std::move(other));
+    swap(temp);
+    return *this;
+  }
+  // clang-format off
+  #endif  // defined(FLATBUFFERS_CPP98_STL)
+  // clang-format on
+
+  ~vector_downward() {
+    clear_buffer();
+    clear_allocator();
+  }
+
+  void reset() {
+    clear_buffer();
+    clear();
+  }
+
+  void clear() {
+    if (buf_) {
+      cur_ = buf_ + reserved_;
+    } else {
+      reserved_ = 0;
+      cur_ = nullptr;
+    }
+    clear_scratch();
+  }
+
+  void clear_scratch() { scratch_ = buf_; }
+
+  void clear_allocator() {
+    if (own_allocator_ && allocator_) { delete allocator_; }
+    allocator_ = nullptr;
+    own_allocator_ = false;
+  }
+
+  void clear_buffer() {
+    if (buf_) Deallocate(allocator_, buf_, reserved_);
+    buf_ = nullptr;
+  }
+
+  // Relinquish the pointer to the caller.
+  uint8_t *release_raw(size_t &allocated_bytes, size_t &offset) {
+    auto *buf = buf_;
+    allocated_bytes = reserved_;
+    offset = static_cast<size_t>(cur_ - buf_);
+
+    // release_raw only relinquishes the buffer ownership.
+    // Does not deallocate or reset the allocator. Destructor will do that.
+    buf_ = nullptr;
+    clear();
+    return buf;
+  }
+
+  // Relinquish the pointer to the caller.
+  DetachedBuffer release() {
+    // allocator ownership (if any) is transferred to DetachedBuffer.
+    DetachedBuffer fb(allocator_, own_allocator_, buf_, reserved_, cur_,
+                      size());
+    if (own_allocator_) {
+      allocator_ = nullptr;
+      own_allocator_ = false;
+    }
+    buf_ = nullptr;
+    clear();
+    return fb;
+  }
+
+  size_t ensure_space(size_t len) {
+    FLATBUFFERS_ASSERT(cur_ >= scratch_ && scratch_ >= buf_);
+    if (len > static_cast<size_t>(cur_ - scratch_)) { reallocate(len); }
+    // Beyond this, signed offsets may not have enough range:
+    // (FlatBuffers > 2GB not supported).
+    FLATBUFFERS_ASSERT(size() < FLATBUFFERS_MAX_BUFFER_SIZE);
+    return len;
+  }
+
+  inline uint8_t *make_space(size_t len) {
+    size_t space = ensure_space(len);
+    cur_ -= space;
+    return cur_;
+  }
+
+  // Returns nullptr if using the DefaultAllocator.
+  Allocator *get_custom_allocator() { return allocator_; }
+
+  uoffset_t size() const {
+    return static_cast<uoffset_t>(reserved_ - (cur_ - buf_));
+  }
+
+  uoffset_t scratch_size() const {
+    return static_cast<uoffset_t>(scratch_ - buf_);
+  }
+
+  size_t capacity() const { return reserved_; }
+
+  uint8_t *data() const {
+    FLATBUFFERS_ASSERT(cur_);
+    return cur_;
+  }
+
+  uint8_t *scratch_data() const {
+    FLATBUFFERS_ASSERT(buf_);
+    return buf_;
+  }
+
+  uint8_t *scratch_end() const {
+    FLATBUFFERS_ASSERT(scratch_);
+    return scratch_;
+  }
+
+  uint8_t *data_at(size_t offset) const { return buf_ + reserved_ - offset; }
+
+  void push(const uint8_t *bytes, size_t num) {
+    if (num > 0) { memcpy(make_space(num), bytes, num); }
+  }
+
+  // Specialized version of push() that avoids memcpy call for small data.
+  template<typename T> void push_small(const T &little_endian_t) {
+    make_space(sizeof(T));
+    *reinterpret_cast<T *>(cur_) = little_endian_t;
+  }
+
+  template<typename T> void scratch_push_small(const T &t) {
+    ensure_space(sizeof(T));
+    *reinterpret_cast<T *>(scratch_) = t;
+    scratch_ += sizeof(T);
+  }
+
+  // fill() is most frequently called with small byte counts (<= 4),
+  // which is why we're using loops rather than calling memset.
+  void fill(size_t zero_pad_bytes) {
+    make_space(zero_pad_bytes);
+    for (size_t i = 0; i < zero_pad_bytes; i++) cur_[i] = 0;
+  }
+
+  // Version for when we know the size is larger.
+  // Precondition: zero_pad_bytes > 0
+  void fill_big(size_t zero_pad_bytes) {
+    memset(make_space(zero_pad_bytes), 0, zero_pad_bytes);
+  }
+
+  void pop(size_t bytes_to_remove) { cur_ += bytes_to_remove; }
+  void scratch_pop(size_t bytes_to_remove) { scratch_ -= bytes_to_remove; }
+
+  void swap(vector_downward &other) {
+    using std::swap;
+    swap(allocator_, other.allocator_);
+    swap(own_allocator_, other.own_allocator_);
+    swap(initial_size_, other.initial_size_);
+    swap(buffer_minalign_, other.buffer_minalign_);
+    swap(reserved_, other.reserved_);
+    swap(buf_, other.buf_);
+    swap(cur_, other.cur_);
+    swap(scratch_, other.scratch_);
+  }
+
+  void swap_allocator(vector_downward &other) {
+    using std::swap;
+    swap(allocator_, other.allocator_);
+    swap(own_allocator_, other.own_allocator_);
+  }
+
+ private:
+  // You shouldn't really be copying instances of this class.
+  FLATBUFFERS_DELETE_FUNC(vector_downward(const vector_downward &))
+  FLATBUFFERS_DELETE_FUNC(vector_downward &operator=(const vector_downward &))
+
+  Allocator *allocator_;
+  bool own_allocator_;
+  size_t initial_size_;
+  size_t buffer_minalign_;
+  size_t reserved_;
+  uint8_t *buf_;
+  uint8_t *cur_;  // Points at location between empty (below) and used (above).
+  uint8_t *scratch_;  // Points to the end of the scratchpad in use.
+
+  void reallocate(size_t len) {
+    auto old_reserved = reserved_;
+    auto old_size = size();
+    auto old_scratch_size = scratch_size();
+    reserved_ +=
+        (std::max)(len, old_reserved ? old_reserved / 2 : initial_size_);
+    reserved_ = (reserved_ + buffer_minalign_ - 1) & ~(buffer_minalign_ - 1);
+    if (buf_) {
+      buf_ = ReallocateDownward(allocator_, buf_, old_reserved, reserved_,
+                                old_size, old_scratch_size);
+    } else {
+      buf_ = Allocate(allocator_, reserved_);
+    }
+    cur_ = buf_ + reserved_ - old_size;
+    scratch_ = buf_ + old_scratch_size;
+  }
+};
+
+// Converts a Field ID to a virtual table offset.
+inline voffset_t FieldIndexToOffset(voffset_t field_id) {
+  // Should correspond to what EndTable() below builds up.
+  const int fixed_fields = 2;  // Vtable size and Object Size.
+  return static_cast<voffset_t>((field_id + fixed_fields) * sizeof(voffset_t));
+}
+
+template<typename T, typename Alloc>
+const T *data(const std::vector<T, Alloc> &v) {
+  // Eventually the returned pointer gets passed down to memcpy, so
+  // we need it to be non-null to avoid undefined behavior.
+  static uint8_t t;
+  return v.empty() ? reinterpret_cast<const T *>(&t) : &v.front();
+}
+template<typename T, typename Alloc> T *data(std::vector<T, Alloc> &v) {
+  // Eventually the returned pointer gets passed down to memcpy, so
+  // we need it to be non-null to avoid undefined behavior.
+  static uint8_t t;
+  return v.empty() ? reinterpret_cast<T *>(&t) : &v.front();
+}
+
+/// @endcond
+
+/// @addtogroup flatbuffers_cpp_api
+/// @{
+/// @class FlatBufferBuilder
+/// @brief Helper class to hold data needed in creation of a FlatBuffer.
+/// To serialize data, you typically call one of the `Create*()` functions in
+/// the generated code, which in turn call a sequence of `StartTable`/
+/// `PushElement`/`AddElement`/`EndTable`, or the builtin `CreateString`/
+/// `CreateVector` functions. Do this is depth-first order to build up a tree to
+/// the root. `Finish()` wraps up the buffer ready for transport.
+class FlatBufferBuilder {
+ public:
+  /// @brief Default constructor for FlatBufferBuilder.
+  /// @param[in] initial_size The initial size of the buffer, in bytes. Defaults
+  /// to `1024`.
+  /// @param[in] allocator An `Allocator` to use. If null will use
+  /// `DefaultAllocator`.
+  /// @param[in] own_allocator Whether the builder/vector should own the
+  /// allocator. Defaults to / `false`.
+  /// @param[in] buffer_minalign Force the buffer to be aligned to the given
+  /// minimum alignment upon reallocation. Only needed if you intend to store
+  /// types with custom alignment AND you wish to read the buffer in-place
+  /// directly after creation.
+  explicit FlatBufferBuilder(
+      size_t initial_size = 1024, Allocator *allocator = nullptr,
+      bool own_allocator = false,
+      size_t buffer_minalign = AlignOf<largest_scalar_t>())
+      : buf_(initial_size, allocator, own_allocator, buffer_minalign),
+        num_field_loc(0),
+        max_voffset_(0),
+        nested(false),
+        finished(false),
+        minalign_(1),
+        force_defaults_(false),
+        dedup_vtables_(true),
+        string_pool(nullptr) {
+    EndianCheck();
+  }
+
+  // clang-format off
+  /// @brief Move constructor for FlatBufferBuilder.
+  #if !defined(FLATBUFFERS_CPP98_STL)
+  FlatBufferBuilder(FlatBufferBuilder &&other)
+  #else
+  FlatBufferBuilder(FlatBufferBuilder &other)
+  #endif  // #if !defined(FLATBUFFERS_CPP98_STL)
+    : buf_(1024, nullptr, false, AlignOf<largest_scalar_t>()),
+      num_field_loc(0),
+      max_voffset_(0),
+      nested(false),
+      finished(false),
+      minalign_(1),
+      force_defaults_(false),
+      dedup_vtables_(true),
+      string_pool(nullptr) {
+    EndianCheck();
+    // Default construct and swap idiom.
+    // Lack of delegating constructors in vs2010 makes it more verbose than needed.
+    Swap(other);
+  }
+  // clang-format on
+
+  // clang-format off
+  #if !defined(FLATBUFFERS_CPP98_STL)
+  // clang-format on
+  /// @brief Move assignment operator for FlatBufferBuilder.
+  FlatBufferBuilder &operator=(FlatBufferBuilder &&other) {
+    // Move construct a temporary and swap idiom
+    FlatBufferBuilder temp(std::move(other));
+    Swap(temp);
+    return *this;
+  }
+  // clang-format off
+  #endif  // defined(FLATBUFFERS_CPP98_STL)
+  // clang-format on
+
+  void Swap(FlatBufferBuilder &other) {
+    using std::swap;
+    buf_.swap(other.buf_);
+    swap(num_field_loc, other.num_field_loc);
+    swap(max_voffset_, other.max_voffset_);
+    swap(nested, other.nested);
+    swap(finished, other.finished);
+    swap(minalign_, other.minalign_);
+    swap(force_defaults_, other.force_defaults_);
+    swap(dedup_vtables_, other.dedup_vtables_);
+    swap(string_pool, other.string_pool);
+  }
+
+  ~FlatBufferBuilder() {
+    if (string_pool) delete string_pool;
+  }
+
+  void Reset() {
+    Clear();       // clear builder state
+    buf_.reset();  // deallocate buffer
+  }
+
+  /// @brief Reset all the state in this FlatBufferBuilder so it can be reused
+  /// to construct another buffer.
+  void Clear() {
+    ClearOffsets();
+    buf_.clear();
+    nested = false;
+    finished = false;
+    minalign_ = 1;
+    if (string_pool) string_pool->clear();
+  }
+
+  /// @brief The current size of the serialized buffer, counting from the end.
+  /// @return Returns an `uoffset_t` with the current size of the buffer.
+  uoffset_t GetSize() const { return buf_.size(); }
+
+  /// @brief Get the serialized buffer (after you call `Finish()`).
+  /// @return Returns an `uint8_t` pointer to the FlatBuffer data inside the
+  /// buffer.
+  uint8_t *GetBufferPointer() const {
+    Finished();
+    return buf_.data();
+  }
+
+  /// @brief Get a pointer to an unfinished buffer.
+  /// @return Returns a `uint8_t` pointer to the unfinished buffer.
+  uint8_t *GetCurrentBufferPointer() const { return buf_.data(); }
+
+  /// @brief Get the released pointer to the serialized buffer.
+  /// @warning Do NOT attempt to use this FlatBufferBuilder afterwards!
+  /// @return A `FlatBuffer` that owns the buffer and its allocator and
+  /// behaves similar to a `unique_ptr` with a deleter.
+  FLATBUFFERS_ATTRIBUTE(deprecated("use Release() instead"))
+  DetachedBuffer ReleaseBufferPointer() {
+    Finished();
+    return buf_.release();
+  }
+
+  /// @brief Get the released DetachedBuffer.
+  /// @return A `DetachedBuffer` that owns the buffer and its allocator.
+  DetachedBuffer Release() {
+    Finished();
+    return buf_.release();
+  }
+
+  /// @brief Get the released pointer to the serialized buffer.
+  /// @param size The size of the memory block containing
+  /// the serialized `FlatBuffer`.
+  /// @param offset The offset from the released pointer where the finished
+  /// `FlatBuffer` starts.
+  /// @return A raw pointer to the start of the memory block containing
+  /// the serialized `FlatBuffer`.
+  /// @remark If the allocator is owned, it gets deleted when the destructor is
+  /// called..
+  uint8_t *ReleaseRaw(size_t &size, size_t &offset) {
+    Finished();
+    return buf_.release_raw(size, offset);
+  }
+
+  /// @brief get the minimum alignment this buffer needs to be accessed
+  /// properly. This is only known once all elements have been written (after
+  /// you call Finish()). You can use this information if you need to embed
+  /// a FlatBuffer in some other buffer, such that you can later read it
+  /// without first having to copy it into its own buffer.
+  size_t GetBufferMinAlignment() {
+    Finished();
+    return minalign_;
+  }
+
+  /// @cond FLATBUFFERS_INTERNAL
+  void Finished() const {
+    // If you get this assert, you're attempting to get access a buffer
+    // which hasn't been finished yet. Be sure to call
+    // FlatBufferBuilder::Finish with your root table.
+    // If you really need to access an unfinished buffer, call
+    // GetCurrentBufferPointer instead.
+    FLATBUFFERS_ASSERT(finished);
+  }
+  /// @endcond
+
+  /// @brief In order to save space, fields that are set to their default value
+  /// don't get serialized into the buffer.
+  /// @param[in] fd When set to `true`, always serializes default values that
+  /// are set. Optional fields which are not set explicitly, will still not be
+  /// serialized.
+  void ForceDefaults(bool fd) { force_defaults_ = fd; }
+
+  /// @brief By default vtables are deduped in order to save space.
+  /// @param[in] dedup When set to `true`, dedup vtables.
+  void DedupVtables(bool dedup) { dedup_vtables_ = dedup; }
+
+  /// @cond FLATBUFFERS_INTERNAL
+  void Pad(size_t num_bytes) { buf_.fill(num_bytes); }
+
+  void TrackMinAlign(size_t elem_size) {
+    if (elem_size > minalign_) minalign_ = elem_size;
+  }
+
+  void Align(size_t elem_size) {
+    TrackMinAlign(elem_size);
+    buf_.fill(PaddingBytes(buf_.size(), elem_size));
+  }
+
+  void PushFlatBuffer(const uint8_t *bytes, size_t size) {
+    PushBytes(bytes, size);
+    finished = true;
+  }
+
+  void PushBytes(const uint8_t *bytes, size_t size) { buf_.push(bytes, size); }
+
+  void PopBytes(size_t amount) { buf_.pop(amount); }
+
+  template<typename T> void AssertScalarT() {
+    // The code assumes power of 2 sizes and endian-swap-ability.
+    static_assert(flatbuffers::is_scalar<T>::value, "T must be a scalar type");
+  }
+
+  // Write a single aligned scalar to the buffer
+  template<typename T> uoffset_t PushElement(T element) {
+    AssertScalarT<T>();
+    T litle_endian_element = EndianScalar(element);
+    Align(sizeof(T));
+    buf_.push_small(litle_endian_element);
+    return GetSize();
+  }
+
+  template<typename T> uoffset_t PushElement(Offset<T> off) {
+    // Special case for offsets: see ReferTo below.
+    return PushElement(ReferTo(off.o));
+  }
+
+  // When writing fields, we track where they are, so we can create correct
+  // vtables later.
+  void TrackField(voffset_t field, uoffset_t off) {
+    FieldLoc fl = { off, field };
+    buf_.scratch_push_small(fl);
+    num_field_loc++;
+    max_voffset_ = (std::max)(max_voffset_, field);
+  }
+
+  // Like PushElement, but additionally tracks the field this represents.
+  template<typename T> void AddElement(voffset_t field, T e, T def) {
+    // We don't serialize values equal to the default.
+    if (IsTheSameAs(e, def) && !force_defaults_) return;
+    auto off = PushElement(e);
+    TrackField(field, off);
+  }
+
+  template<typename T> void AddOffset(voffset_t field, Offset<T> off) {
+    if (off.IsNull()) return;  // Don't store.
+    AddElement(field, ReferTo(off.o), static_cast<uoffset_t>(0));
+  }
+
+  template<typename T> void AddStruct(voffset_t field, const T *structptr) {
+    if (!structptr) return;  // Default, don't store.
+    Align(AlignOf<T>());
+    buf_.push_small(*structptr);
+    TrackField(field, GetSize());
+  }
+
+  void AddStructOffset(voffset_t field, uoffset_t off) {
+    TrackField(field, off);
+  }
+
+  // Offsets initially are relative to the end of the buffer (downwards).
+  // This function converts them to be relative to the current location
+  // in the buffer (when stored here), pointing upwards.
+  uoffset_t ReferTo(uoffset_t off) {
+    // Align to ensure GetSize() below is correct.
+    Align(sizeof(uoffset_t));
+    // Offset must refer to something already in buffer.
+    FLATBUFFERS_ASSERT(off && off <= GetSize());
+    return GetSize() - off + static_cast<uoffset_t>(sizeof(uoffset_t));
+  }
+
+  void NotNested() {
+    // If you hit this, you're trying to construct a Table/Vector/String
+    // during the construction of its parent table (between the MyTableBuilder
+    // and table.Finish().
+    // Move the creation of these sub-objects to above the MyTableBuilder to
+    // not get this assert.
+    // Ignoring this assert may appear to work in simple cases, but the reason
+    // it is here is that storing objects in-line may cause vtable offsets
+    // to not fit anymore. It also leads to vtable duplication.
+    FLATBUFFERS_ASSERT(!nested);
+    // If you hit this, fields were added outside the scope of a table.
+    FLATBUFFERS_ASSERT(!num_field_loc);
+  }
+
+  // From generated code (or from the parser), we call StartTable/EndTable
+  // with a sequence of AddElement calls in between.
+  uoffset_t StartTable() {
+    NotNested();
+    nested = true;
+    return GetSize();
+  }
+
+  // This finishes one serialized object by generating the vtable if it's a
+  // table, comparing it against existing vtables, and writing the
+  // resulting vtable offset.
+  uoffset_t EndTable(uoffset_t start) {
+    // If you get this assert, a corresponding StartTable wasn't called.
+    FLATBUFFERS_ASSERT(nested);
+    // Write the vtable offset, which is the start of any Table.
+    // We fill it's value later.
+    auto vtableoffsetloc = PushElement<soffset_t>(0);
+    // Write a vtable, which consists entirely of voffset_t elements.
+    // It starts with the number of offsets, followed by a type id, followed
+    // by the offsets themselves. In reverse:
+    // Include space for the last offset and ensure empty tables have a
+    // minimum size.
+    max_voffset_ =
+        (std::max)(static_cast<voffset_t>(max_voffset_ + sizeof(voffset_t)),
+                   FieldIndexToOffset(0));
+    buf_.fill_big(max_voffset_);
+    auto table_object_size = vtableoffsetloc - start;
+    // Vtable use 16bit offsets.
+    FLATBUFFERS_ASSERT(table_object_size < 0x10000);
+    WriteScalar<voffset_t>(buf_.data() + sizeof(voffset_t),
+                           static_cast<voffset_t>(table_object_size));
+    WriteScalar<voffset_t>(buf_.data(), max_voffset_);
+    // Write the offsets into the table
+    for (auto it = buf_.scratch_end() - num_field_loc * sizeof(FieldLoc);
+         it < buf_.scratch_end(); it += sizeof(FieldLoc)) {
+      auto field_location = reinterpret_cast<FieldLoc *>(it);
+      auto pos = static_cast<voffset_t>(vtableoffsetloc - field_location->off);
+      // If this asserts, it means you've set a field twice.
+      FLATBUFFERS_ASSERT(
+          !ReadScalar<voffset_t>(buf_.data() + field_location->id));
+      WriteScalar<voffset_t>(buf_.data() + field_location->id, pos);
+    }
+    ClearOffsets();
+    auto vt1 = reinterpret_cast<voffset_t *>(buf_.data());
+    auto vt1_size = ReadScalar<voffset_t>(vt1);
+    auto vt_use = GetSize();
+    // See if we already have generated a vtable with this exact same
+    // layout before. If so, make it point to the old one, remove this one.
+    if (dedup_vtables_) {
+      for (auto it = buf_.scratch_data(); it < buf_.scratch_end();
+           it += sizeof(uoffset_t)) {
+        auto vt_offset_ptr = reinterpret_cast<uoffset_t *>(it);
+        auto vt2 = reinterpret_cast<voffset_t *>(buf_.data_at(*vt_offset_ptr));
+        auto vt2_size = *vt2;
+        if (vt1_size != vt2_size || 0 != memcmp(vt2, vt1, vt1_size)) continue;
+        vt_use = *vt_offset_ptr;
+        buf_.pop(GetSize() - vtableoffsetloc);
+        break;
+      }
+    }
+    // If this is a new vtable, remember it.
+    if (vt_use == GetSize()) { buf_.scratch_push_small(vt_use); }
+    // Fill the vtable offset we created above.
+    // The offset points from the beginning of the object to where the
+    // vtable is stored.
+    // Offsets default direction is downward in memory for future format
+    // flexibility (storing all vtables at the start of the file).
+    WriteScalar(buf_.data_at(vtableoffsetloc),
+                static_cast<soffset_t>(vt_use) -
+                    static_cast<soffset_t>(vtableoffsetloc));
+
+    nested = false;
+    return vtableoffsetloc;
+  }
+
+  FLATBUFFERS_ATTRIBUTE(deprecated("call the version above instead"))
+  uoffset_t EndTable(uoffset_t start, voffset_t /*numfields*/) {
+    return EndTable(start);
+  }
+
+  // This checks a required field has been set in a given table that has
+  // just been constructed.
+  template<typename T> void Required(Offset<T> table, voffset_t field);
+
+  uoffset_t StartStruct(size_t alignment) {
+    Align(alignment);
+    return GetSize();
+  }
+
+  uoffset_t EndStruct() { return GetSize(); }
+
+  void ClearOffsets() {
+    buf_.scratch_pop(num_field_loc * sizeof(FieldLoc));
+    num_field_loc = 0;
+    max_voffset_ = 0;
+  }
+
+  // Aligns such that when "len" bytes are written, an object can be written
+  // after it with "alignment" without padding.
+  void PreAlign(size_t len, size_t alignment) {
+    TrackMinAlign(alignment);
+    buf_.fill(PaddingBytes(GetSize() + len, alignment));
+  }
+  template<typename T> void PreAlign(size_t len) {
+    AssertScalarT<T>();
+    PreAlign(len, sizeof(T));
+  }
+  /// @endcond
+
+  /// @brief Store a string in the buffer, which can contain any binary data.
+  /// @param[in] str A const char pointer to the data to be stored as a string.
+  /// @param[in] len The number of bytes that should be stored from `str`.
+  /// @return Returns the offset in the buffer where the string starts.
+  Offset<String> CreateString(const char *str, size_t len) {
+    NotNested();
+    PreAlign<uoffset_t>(len + 1);  // Always 0-terminated.
+    buf_.fill(1);
+    PushBytes(reinterpret_cast<const uint8_t *>(str), len);
+    PushElement(static_cast<uoffset_t>(len));
+    return Offset<String>(GetSize());
+  }
+
+  /// @brief Store a string in the buffer, which is null-terminated.
+  /// @param[in] str A const char pointer to a C-string to add to the buffer.
+  /// @return Returns the offset in the buffer where the string starts.
+  Offset<String> CreateString(const char *str) {
+    return CreateString(str, strlen(str));
+  }
+
+  /// @brief Store a string in the buffer, which is null-terminated.
+  /// @param[in] str A char pointer to a C-string to add to the buffer.
+  /// @return Returns the offset in the buffer where the string starts.
+  Offset<String> CreateString(char *str) {
+    return CreateString(str, strlen(str));
+  }
+
+  /// @brief Store a string in the buffer, which can contain any binary data.
+  /// @param[in] str A const reference to a std::string to store in the buffer.
+  /// @return Returns the offset in the buffer where the string starts.
+  Offset<String> CreateString(const std::string &str) {
+    return CreateString(str.c_str(), str.length());
+  }
+
+  // clang-format off
+  #ifdef FLATBUFFERS_HAS_STRING_VIEW
+  /// @brief Store a string in the buffer, which can contain any binary data.
+  /// @param[in] str A const string_view to copy in to the buffer.
+  /// @return Returns the offset in the buffer where the string starts.
+  Offset<String> CreateString(flatbuffers::string_view str) {
+    return CreateString(str.data(), str.size());
+  }
+  #endif // FLATBUFFERS_HAS_STRING_VIEW
+  // clang-format on
+
+  /// @brief Store a string in the buffer, which can contain any binary data.
+  /// @param[in] str A const pointer to a `String` struct to add to the buffer.
+  /// @return Returns the offset in the buffer where the string starts
+  Offset<String> CreateString(const String *str) {
+    return str ? CreateString(str->c_str(), str->size()) : 0;
+  }
+
+  /// @brief Store a string in the buffer, which can contain any binary data.
+  /// @param[in] str A const reference to a std::string like type with support
+  /// of T::c_str() and T::length() to store in the buffer.
+  /// @return Returns the offset in the buffer where the string starts.
+  template<typename T> Offset<String> CreateString(const T &str) {
+    return CreateString(str.c_str(), str.length());
+  }
+
+  /// @brief Store a string in the buffer, which can contain any binary data.
+  /// If a string with this exact contents has already been serialized before,
+  /// instead simply returns the offset of the existing string.
+  /// @param[in] str A const char pointer to the data to be stored as a string.
+  /// @param[in] len The number of bytes that should be stored from `str`.
+  /// @return Returns the offset in the buffer where the string starts.
+  Offset<String> CreateSharedString(const char *str, size_t len) {
+    if (!string_pool)
+      string_pool = new StringOffsetMap(StringOffsetCompare(buf_));
+    auto size_before_string = buf_.size();
+    // Must first serialize the string, since the set is all offsets into
+    // buffer.
+    auto off = CreateString(str, len);
+    auto it = string_pool->find(off);
+    // If it exists we reuse existing serialized data!
+    if (it != string_pool->end()) {
+      // We can remove the string we serialized.
+      buf_.pop(buf_.size() - size_before_string);
+      return *it;
+    }
+    // Record this string for future use.
+    string_pool->insert(off);
+    return off;
+  }
+
+  /// @brief Store a string in the buffer, which null-terminated.
+  /// If a string with this exact contents has already been serialized before,
+  /// instead simply returns the offset of the existing string.
+  /// @param[in] str A const char pointer to a C-string to add to the buffer.
+  /// @return Returns the offset in the buffer where the string starts.
+  Offset<String> CreateSharedString(const char *str) {
+    return CreateSharedString(str, strlen(str));
+  }
+
+  /// @brief Store a string in the buffer, which can contain any binary data.
+  /// If a string with this exact contents has already been serialized before,
+  /// instead simply returns the offset of the existing string.
+  /// @param[in] str A const reference to a std::string to store in the buffer.
+  /// @return Returns the offset in the buffer where the string starts.
+  Offset<String> CreateSharedString(const std::string &str) {
+    return CreateSharedString(str.c_str(), str.length());
+  }
+
+  /// @brief Store a string in the buffer, which can contain any binary data.
+  /// If a string with this exact contents has already been serialized before,
+  /// instead simply returns the offset of the existing string.
+  /// @param[in] str A const pointer to a `String` struct to add to the buffer.
+  /// @return Returns the offset in the buffer where the string starts
+  Offset<String> CreateSharedString(const String *str) {
+    return CreateSharedString(str->c_str(), str->size());
+  }
+
+  /// @cond FLATBUFFERS_INTERNAL
+  uoffset_t EndVector(size_t len) {
+    FLATBUFFERS_ASSERT(nested);  // Hit if no corresponding StartVector.
+    nested = false;
+    return PushElement(static_cast<uoffset_t>(len));
+  }
+
+  void StartVector(size_t len, size_t elemsize) {
+    NotNested();
+    nested = true;
+    PreAlign<uoffset_t>(len * elemsize);
+    PreAlign(len * elemsize, elemsize);  // Just in case elemsize > uoffset_t.
+  }
+
+  // Call this right before StartVector/CreateVector if you want to force the
+  // alignment to be something different than what the element size would
+  // normally dictate.
+  // This is useful when storing a nested_flatbuffer in a vector of bytes,
+  // or when storing SIMD floats, etc.
+  void ForceVectorAlignment(size_t len, size_t elemsize, size_t alignment) {
+    PreAlign(len * elemsize, alignment);
+  }
+
+  // Similar to ForceVectorAlignment but for String fields.
+  void ForceStringAlignment(size_t len, size_t alignment) {
+    PreAlign((len + 1) * sizeof(char), alignment);
+  }
+
+  /// @endcond
+
+  /// @brief Serialize an array into a FlatBuffer `vector`.
+  /// @tparam T The data type of the array elements.
+  /// @param[in] v A pointer to the array of type `T` to serialize into the
+  /// buffer as a `vector`.
+  /// @param[in] len The number of elements to serialize.
+  /// @return Returns a typed `Offset` into the serialized data indicating
+  /// where the vector is stored.
+  template<typename T> Offset<Vector<T>> CreateVector(const T *v, size_t len) {
+    // If this assert hits, you're specifying a template argument that is
+    // causing the wrong overload to be selected, remove it.
+    AssertScalarT<T>();
+    StartVector(len, sizeof(T));
+    // clang-format off
+    #if FLATBUFFERS_LITTLEENDIAN
+      PushBytes(reinterpret_cast<const uint8_t *>(v), len * sizeof(T));
+    #else
+      if (sizeof(T) == 1) {
+        PushBytes(reinterpret_cast<const uint8_t *>(v), len);
+      } else {
+        for (auto i = len; i > 0; ) {
+          PushElement(v[--i]);
+        }
+      }
+    #endif
+    // clang-format on
+    return Offset<Vector<T>>(EndVector(len));
+  }
+
+  template<typename T>
+  Offset<Vector<Offset<T>>> CreateVector(const Offset<T> *v, size_t len) {
+    StartVector(len, sizeof(Offset<T>));
+    for (auto i = len; i > 0;) { PushElement(v[--i]); }
+    return Offset<Vector<Offset<T>>>(EndVector(len));
+  }
+
+  /// @brief Serialize a `std::vector` into a FlatBuffer `vector`.
+  /// @tparam T The data type of the `std::vector` elements.
+  /// @param v A const reference to the `std::vector` to serialize into the
+  /// buffer as a `vector`.
+  /// @return Returns a typed `Offset` into the serialized data indicating
+  /// where the vector is stored.
+  template<typename T> Offset<Vector<T>> CreateVector(const std::vector<T> &v) {
+    return CreateVector(data(v), v.size());
+  }
+
+  // vector<bool> may be implemented using a bit-set, so we can't access it as
+  // an array. Instead, read elements manually.
+  // Background: https://isocpp.org/blog/2012/11/on-vectorbool
+  Offset<Vector<uint8_t>> CreateVector(const std::vector<bool> &v) {
+    StartVector(v.size(), sizeof(uint8_t));
+    for (auto i = v.size(); i > 0;) {
+      PushElement(static_cast<uint8_t>(v[--i]));
+    }
+    return Offset<Vector<uint8_t>>(EndVector(v.size()));
+  }
+
+  // clang-format off
+  #ifndef FLATBUFFERS_CPP98_STL
+  /// @brief Serialize values returned by a function into a FlatBuffer `vector`.
+  /// This is a convenience function that takes care of iteration for you.
+  /// @tparam T The data type of the `std::vector` elements.
+  /// @param f A function that takes the current iteration 0..vector_size-1 and
+  /// returns any type that you can construct a FlatBuffers vector out of.
+  /// @return Returns a typed `Offset` into the serialized data indicating
+  /// where the vector is stored.
+  template<typename T> Offset<Vector<T>> CreateVector(size_t vector_size,
+      const std::function<T (size_t i)> &f) {
+    std::vector<T> elems(vector_size);
+    for (size_t i = 0; i < vector_size; i++) elems[i] = f(i);
+    return CreateVector(elems);
+  }
+  #endif
+  // clang-format on
+
+  /// @brief Serialize values returned by a function into a FlatBuffer `vector`.
+  /// This is a convenience function that takes care of iteration for you.
+  /// @tparam T The data type of the `std::vector` elements.
+  /// @param f A function that takes the current iteration 0..vector_size-1,
+  /// and the state parameter returning any type that you can construct a
+  /// FlatBuffers vector out of.
+  /// @param state State passed to f.
+  /// @return Returns a typed `Offset` into the serialized data indicating
+  /// where the vector is stored.
+  template<typename T, typename F, typename S>
+  Offset<Vector<T>> CreateVector(size_t vector_size, F f, S *state) {
+    std::vector<T> elems(vector_size);
+    for (size_t i = 0; i < vector_size; i++) elems[i] = f(i, state);
+    return CreateVector(elems);
+  }
+
+  /// @brief Serialize a `std::vector<std::string>` into a FlatBuffer `vector`.
+  /// This is a convenience function for a common case.
+  /// @param v A const reference to the `std::vector` to serialize into the
+  /// buffer as a `vector`.
+  /// @return Returns a typed `Offset` into the serialized data indicating
+  /// where the vector is stored.
+  Offset<Vector<Offset<String>>> CreateVectorOfStrings(
+      const std::vector<std::string> &v) {
+    std::vector<Offset<String>> offsets(v.size());
+    for (size_t i = 0; i < v.size(); i++) offsets[i] = CreateString(v[i]);
+    return CreateVector(offsets);
+  }
+
+  /// @brief Serialize an array of structs into a FlatBuffer `vector`.
+  /// @tparam T The data type of the struct array elements.
+  /// @param[in] v A pointer to the array of type `T` to serialize into the
+  /// buffer as a `vector`.
+  /// @param[in] len The number of elements to serialize.
+  /// @return Returns a typed `Offset` into the serialized data indicating
+  /// where the vector is stored.
+  template<typename T>
+  Offset<Vector<const T *>> CreateVectorOfStructs(const T *v, size_t len) {
+    StartVector(len * sizeof(T) / AlignOf<T>(), AlignOf<T>());
+    PushBytes(reinterpret_cast<const uint8_t *>(v), sizeof(T) * len);
+    return Offset<Vector<const T *>>(EndVector(len));
+  }
+
+  /// @brief Serialize an array of native structs into a FlatBuffer `vector`.
+  /// @tparam T The data type of the struct array elements.
+  /// @tparam S The data type of the native struct array elements.
+  /// @param[in] v A pointer to the array of type `S` to serialize into the
+  /// buffer as a `vector`.
+  /// @param[in] len The number of elements to serialize.
+  /// @return Returns a typed `Offset` into the serialized data indicating
+  /// where the vector is stored.
+  template<typename T, typename S>
+  Offset<Vector<const T *>> CreateVectorOfNativeStructs(const S *v,
+                                                        size_t len) {
+    extern T Pack(const S &);
+    std::vector<T> vv(len);
+    std::transform(v, v + len, vv.begin(), Pack);
+    return CreateVectorOfStructs<T>(data(vv), vv.size());
+  }
+
+  // clang-format off
+  #ifndef FLATBUFFERS_CPP98_STL
+  /// @brief Serialize an array of structs into a FlatBuffer `vector`.
+  /// @tparam T The data type of the struct array elements.
+  /// @param[in] filler A function that takes the current iteration 0..vector_size-1
+  /// and a pointer to the struct that must be filled.
+  /// @return Returns a typed `Offset` into the serialized data indicating
+  /// where the vector is stored.
+  /// This is mostly useful when flatbuffers are generated with mutation
+  /// accessors.
+  template<typename T> Offset<Vector<const T *>> CreateVectorOfStructs(
+      size_t vector_size, const std::function<void(size_t i, T *)> &filler) {
+    T* structs = StartVectorOfStructs<T>(vector_size);
+    for (size_t i = 0; i < vector_size; i++) {
+      filler(i, structs);
+      structs++;
+    }
+    return EndVectorOfStructs<T>(vector_size);
+  }
+  #endif
+  // clang-format on
+
+  /// @brief Serialize an array of structs into a FlatBuffer `vector`.
+  /// @tparam T The data type of the struct array elements.
+  /// @param[in] f A function that takes the current iteration 0..vector_size-1,
+  /// a pointer to the struct that must be filled and the state argument.
+  /// @param[in] state Arbitrary state to pass to f.
+  /// @return Returns a typed `Offset` into the serialized data indicating
+  /// where the vector is stored.
+  /// This is mostly useful when flatbuffers are generated with mutation
+  /// accessors.
+  template<typename T, typename F, typename S>
+  Offset<Vector<const T *>> CreateVectorOfStructs(size_t vector_size, F f,
+                                                  S *state) {
+    T *structs = StartVectorOfStructs<T>(vector_size);
+    for (size_t i = 0; i < vector_size; i++) {
+      f(i, structs, state);
+      structs++;
+    }
+    return EndVectorOfStructs<T>(vector_size);
+  }
+
+  /// @brief Serialize a `std::vector` of structs into a FlatBuffer `vector`.
+  /// @tparam T The data type of the `std::vector` struct elements.
+  /// @param[in] v A const reference to the `std::vector` of structs to
+  /// serialize into the buffer as a `vector`.
+  /// @return Returns a typed `Offset` into the serialized data indicating
+  /// where the vector is stored.
+  template<typename T, typename Alloc>
+  Offset<Vector<const T *>> CreateVectorOfStructs(
+      const std::vector<T, Alloc> &v) {
+    return CreateVectorOfStructs(data(v), v.size());
+  }
+
+  /// @brief Serialize a `std::vector` of native structs into a FlatBuffer
+  /// `vector`.
+  /// @tparam T The data type of the `std::vector` struct elements.
+  /// @tparam S The data type of the `std::vector` native struct elements.
+  /// @param[in] v A const reference to the `std::vector` of structs to
+  /// serialize into the buffer as a `vector`.
+  /// @return Returns a typed `Offset` into the serialized data indicating
+  /// where the vector is stored.
+  template<typename T, typename S>
+  Offset<Vector<const T *>> CreateVectorOfNativeStructs(
+      const std::vector<S> &v) {
+    return CreateVectorOfNativeStructs<T, S>(data(v), v.size());
+  }
+
+  /// @cond FLATBUFFERS_INTERNAL
+  template<typename T> struct StructKeyComparator {
+    bool operator()(const T &a, const T &b) const {
+      return a.KeyCompareLessThan(&b);
+    }
+
+   private:
+    StructKeyComparator &operator=(const StructKeyComparator &);
+  };
+  /// @endcond
+
+  /// @brief Serialize a `std::vector` of structs into a FlatBuffer `vector`
+  /// in sorted order.
+  /// @tparam T The data type of the `std::vector` struct elements.
+  /// @param[in] v A const reference to the `std::vector` of structs to
+  /// serialize into the buffer as a `vector`.
+  /// @return Returns a typed `Offset` into the serialized data indicating
+  /// where the vector is stored.
+  template<typename T>
+  Offset<Vector<const T *>> CreateVectorOfSortedStructs(std::vector<T> *v) {
+    return CreateVectorOfSortedStructs(data(*v), v->size());
+  }
+
+  /// @brief Serialize a `std::vector` of native structs into a FlatBuffer
+  /// `vector` in sorted order.
+  /// @tparam T The data type of the `std::vector` struct elements.
+  /// @tparam S The data type of the `std::vector` native struct elements.
+  /// @param[in] v A const reference to the `std::vector` of structs to
+  /// serialize into the buffer as a `vector`.
+  /// @return Returns a typed `Offset` into the serialized data indicating
+  /// where the vector is stored.
+  template<typename T, typename S>
+  Offset<Vector<const T *>> CreateVectorOfSortedNativeStructs(
+      std::vector<S> *v) {
+    return CreateVectorOfSortedNativeStructs<T, S>(data(*v), v->size());
+  }
+
+  /// @brief Serialize an array of structs into a FlatBuffer `vector` in sorted
+  /// order.
+  /// @tparam T The data type of the struct array elements.
+  /// @param[in] v A pointer to the array of type `T` to serialize into the
+  /// buffer as a `vector`.
+  /// @param[in] len The number of elements to serialize.
+  /// @return Returns a typed `Offset` into the serialized data indicating
+  /// where the vector is stored.
+  template<typename T>
+  Offset<Vector<const T *>> CreateVectorOfSortedStructs(T *v, size_t len) {
+    std::sort(v, v + len, StructKeyComparator<T>());
+    return CreateVectorOfStructs(v, len);
+  }
+
+  /// @brief Serialize an array of native structs into a FlatBuffer `vector` in
+  /// sorted order.
+  /// @tparam T The data type of the struct array elements.
+  /// @tparam S The data type of the native struct array elements.
+  /// @param[in] v A pointer to the array of type `S` to serialize into the
+  /// buffer as a `vector`.
+  /// @param[in] len The number of elements to serialize.
+  /// @return Returns a typed `Offset` into the serialized data indicating
+  /// where the vector is stored.
+  template<typename T, typename S>
+  Offset<Vector<const T *>> CreateVectorOfSortedNativeStructs(S *v,
+                                                              size_t len) {
+    extern T Pack(const S &);
+    typedef T (*Pack_t)(const S &);
+    std::vector<T> vv(len);
+    std::transform(v, v + len, vv.begin(), static_cast<Pack_t &>(Pack));
+    return CreateVectorOfSortedStructs<T>(vv, len);
+  }
+
+  /// @cond FLATBUFFERS_INTERNAL
+  template<typename T> struct TableKeyComparator {
+    TableKeyComparator(vector_downward &buf) : buf_(buf) {}
+    bool operator()(const Offset<T> &a, const Offset<T> &b) const {
+      auto table_a = reinterpret_cast<T *>(buf_.data_at(a.o));
+      auto table_b = reinterpret_cast<T *>(buf_.data_at(b.o));
+      return table_a->KeyCompareLessThan(table_b);
+    }
+    vector_downward &buf_;
+
+   private:
+    TableKeyComparator &operator=(const TableKeyComparator &);
+  };
+  /// @endcond
+
+  /// @brief Serialize an array of `table` offsets as a `vector` in the buffer
+  /// in sorted order.
+  /// @tparam T The data type that the offset refers to.
+  /// @param[in] v An array of type `Offset<T>` that contains the `table`
+  /// offsets to store in the buffer in sorted order.
+  /// @param[in] len The number of elements to store in the `vector`.
+  /// @return Returns a typed `Offset` into the serialized data indicating
+  /// where the vector is stored.
+  template<typename T>
+  Offset<Vector<Offset<T>>> CreateVectorOfSortedTables(Offset<T> *v,
+                                                       size_t len) {
+    std::sort(v, v + len, TableKeyComparator<T>(buf_));
+    return CreateVector(v, len);
+  }
+
+  /// @brief Serialize an array of `table` offsets as a `vector` in the buffer
+  /// in sorted order.
+  /// @tparam T The data type that the offset refers to.
+  /// @param[in] v An array of type `Offset<T>` that contains the `table`
+  /// offsets to store in the buffer in sorted order.
+  /// @return Returns a typed `Offset` into the serialized data indicating
+  /// where the vector is stored.
+  template<typename T>
+  Offset<Vector<Offset<T>>> CreateVectorOfSortedTables(
+      std::vector<Offset<T>> *v) {
+    return CreateVectorOfSortedTables(data(*v), v->size());
+  }
+
+  /// @brief Specialized version of `CreateVector` for non-copying use cases.
+  /// Write the data any time later to the returned buffer pointer `buf`.
+  /// @param[in] len The number of elements to store in the `vector`.
+  /// @param[in] elemsize The size of each element in the `vector`.
+  /// @param[out] buf A pointer to a `uint8_t` pointer that can be
+  /// written to at a later time to serialize the data into a `vector`
+  /// in the buffer.
+  uoffset_t CreateUninitializedVector(size_t len, size_t elemsize,
+                                      uint8_t **buf) {
+    NotNested();
+    StartVector(len, elemsize);
+    buf_.make_space(len * elemsize);
+    auto vec_start = GetSize();
+    auto vec_end = EndVector(len);
+    *buf = buf_.data_at(vec_start);
+    return vec_end;
+  }
+
+  /// @brief Specialized version of `CreateVector` for non-copying use cases.
+  /// Write the data any time later to the returned buffer pointer `buf`.
+  /// @tparam T The data type of the data that will be stored in the buffer
+  /// as a `vector`.
+  /// @param[in] len The number of elements to store in the `vector`.
+  /// @param[out] buf A pointer to a pointer of type `T` that can be
+  /// written to at a later time to serialize the data into a `vector`
+  /// in the buffer.
+  template<typename T>
+  Offset<Vector<T>> CreateUninitializedVector(size_t len, T **buf) {
+    AssertScalarT<T>();
+    return CreateUninitializedVector(len, sizeof(T),
+                                     reinterpret_cast<uint8_t **>(buf));
+  }
+
+  template<typename T>
+  Offset<Vector<const T *>> CreateUninitializedVectorOfStructs(size_t len,
+                                                               T **buf) {
+    return CreateUninitializedVector(len, sizeof(T),
+                                     reinterpret_cast<uint8_t **>(buf));
+  }
+
+  // @brief Create a vector of scalar type T given as input a vector of scalar
+  // type U, useful with e.g. pre "enum class" enums, or any existing scalar
+  // data of the wrong type.
+  template<typename T, typename U>
+  Offset<Vector<T>> CreateVectorScalarCast(const U *v, size_t len) {
+    AssertScalarT<T>();
+    AssertScalarT<U>();
+    StartVector(len, sizeof(T));
+    for (auto i = len; i > 0;) { PushElement(static_cast<T>(v[--i])); }
+    return Offset<Vector<T>>(EndVector(len));
+  }
+
+  /// @brief Write a struct by itself, typically to be part of a union.
+  template<typename T> Offset<const T *> CreateStruct(const T &structobj) {
+    NotNested();
+    Align(AlignOf<T>());
+    buf_.push_small(structobj);
+    return Offset<const T *>(GetSize());
+  }
+
+  /// @brief The length of a FlatBuffer file header.
+  static const size_t kFileIdentifierLength = 4;
+
+  /// @brief Finish serializing a buffer by writing the root offset.
+  /// @param[in] file_identifier If a `file_identifier` is given, the buffer
+  /// will be prefixed with a standard FlatBuffers file header.
+  template<typename T>
+  void Finish(Offset<T> root, const char *file_identifier = nullptr) {
+    Finish(root.o, file_identifier, false);
+  }
+
+  /// @brief Finish a buffer with a 32 bit size field pre-fixed (size of the
+  /// buffer following the size field). These buffers are NOT compatible
+  /// with standard buffers created by Finish, i.e. you can't call GetRoot
+  /// on them, you have to use GetSizePrefixedRoot instead.
+  /// All >32 bit quantities in this buffer will be aligned when the whole
+  /// size pre-fixed buffer is aligned.
+  /// These kinds of buffers are useful for creating a stream of FlatBuffers.
+  template<typename T>
+  void FinishSizePrefixed(Offset<T> root,
+                          const char *file_identifier = nullptr) {
+    Finish(root.o, file_identifier, true);
+  }
+
+  void SwapBufAllocator(FlatBufferBuilder &other) {
+    buf_.swap_allocator(other.buf_);
+  }
+
+ protected:
+  // You shouldn't really be copying instances of this class.
+  FlatBufferBuilder(const FlatBufferBuilder &);
+  FlatBufferBuilder &operator=(const FlatBufferBuilder &);
+
+  void Finish(uoffset_t root, const char *file_identifier, bool size_prefix) {
+    NotNested();
+    buf_.clear_scratch();
+    // This will cause the whole buffer to be aligned.
+    PreAlign((size_prefix ? sizeof(uoffset_t) : 0) + sizeof(uoffset_t) +
+                 (file_identifier ? kFileIdentifierLength : 0),
+             minalign_);
+    if (file_identifier) {
+      FLATBUFFERS_ASSERT(strlen(file_identifier) == kFileIdentifierLength);
+      PushBytes(reinterpret_cast<const uint8_t *>(file_identifier),
+                kFileIdentifierLength);
+    }
+    PushElement(ReferTo(root));  // Location of root.
+    if (size_prefix) { PushElement(GetSize()); }
+    finished = true;
+  }
+
+  struct FieldLoc {
+    uoffset_t off;
+    voffset_t id;
+  };
+
+  vector_downward buf_;
+
+  // Accumulating offsets of table members while it is being built.
+  // We store these in the scratch pad of buf_, after the vtable offsets.
+  uoffset_t num_field_loc;
+  // Track how much of the vtable is in use, so we can output the most compact
+  // possible vtable.
+  voffset_t max_voffset_;
+
+  // Ensure objects are not nested.
+  bool nested;
+
+  // Ensure the buffer is finished before it is being accessed.
+  bool finished;
+
+  size_t minalign_;
+
+  bool force_defaults_;  // Serialize values equal to their defaults anyway.
+
+  bool dedup_vtables_;
+
+  struct StringOffsetCompare {
+    StringOffsetCompare(const vector_downward &buf) : buf_(&buf) {}
+    bool operator()(const Offset<String> &a, const Offset<String> &b) const {
+      auto stra = reinterpret_cast<const String *>(buf_->data_at(a.o));
+      auto strb = reinterpret_cast<const String *>(buf_->data_at(b.o));
+      return StringLessThan(stra->data(), stra->size(), strb->data(),
+                            strb->size());
+    }
+    const vector_downward *buf_;
+  };
+
+  // For use with CreateSharedString. Instantiated on first use only.
+  typedef std::set<Offset<String>, StringOffsetCompare> StringOffsetMap;
+  StringOffsetMap *string_pool;
+
+ private:
+  // Allocates space for a vector of structures.
+  // Must be completed with EndVectorOfStructs().
+  template<typename T> T *StartVectorOfStructs(size_t vector_size) {
+    StartVector(vector_size * sizeof(T) / AlignOf<T>(), AlignOf<T>());
+    return reinterpret_cast<T *>(buf_.make_space(vector_size * sizeof(T)));
+  }
+
+  // End the vector of structues in the flatbuffers.
+  // Vector should have previously be started with StartVectorOfStructs().
+  template<typename T>
+  Offset<Vector<const T *>> EndVectorOfStructs(size_t vector_size) {
+    return Offset<Vector<const T *>>(EndVector(vector_size));
+  }
+};
+/// @}
+
+/// @cond FLATBUFFERS_INTERNAL
+// Helpers to get a typed pointer to the root object contained in the buffer.
+template<typename T> T *GetMutableRoot(void *buf) {
+  EndianCheck();
+  return reinterpret_cast<T *>(
+      reinterpret_cast<uint8_t *>(buf) +
+      EndianScalar(*reinterpret_cast<uoffset_t *>(buf)));
+}
+
+template<typename T> const T *GetRoot(const void *buf) {
+  return GetMutableRoot<T>(const_cast<void *>(buf));
+}
+
+template<typename T> const T *GetSizePrefixedRoot(const void *buf) {
+  return GetRoot<T>(reinterpret_cast<const uint8_t *>(buf) + sizeof(uoffset_t));
+}
+
+/// Helpers to get a typed pointer to objects that are currently being built.
+/// @warning Creating new objects will lead to reallocations and invalidates
+/// the pointer!
+template<typename T>
+T *GetMutableTemporaryPointer(FlatBufferBuilder &fbb, Offset<T> offset) {
+  return reinterpret_cast<T *>(fbb.GetCurrentBufferPointer() + fbb.GetSize() -
+                               offset.o);
+}
+
+template<typename T>
+const T *GetTemporaryPointer(FlatBufferBuilder &fbb, Offset<T> offset) {
+  return GetMutableTemporaryPointer<T>(fbb, offset);
+}
+
+/// @brief Get a pointer to the the file_identifier section of the buffer.
+/// @return Returns a const char pointer to the start of the file_identifier
+/// characters in the buffer.  The returned char * has length
+/// 'flatbuffers::FlatBufferBuilder::kFileIdentifierLength'.
+/// This function is UNDEFINED for FlatBuffers whose schema does not include
+/// a file_identifier (likely points at padding or the start of a the root
+/// vtable).
+inline const char *GetBufferIdentifier(const void *buf,
+                                       bool size_prefixed = false) {
+  return reinterpret_cast<const char *>(buf) +
+         ((size_prefixed) ? 2 * sizeof(uoffset_t) : sizeof(uoffset_t));
+}
+
+// Helper to see if the identifier in a buffer has the expected value.
+inline bool BufferHasIdentifier(const void *buf, const char *identifier,
+                                bool size_prefixed = false) {
+  return strncmp(GetBufferIdentifier(buf, size_prefixed), identifier,
+                 FlatBufferBuilder::kFileIdentifierLength) == 0;
+}
+
+// Helper class to verify the integrity of a FlatBuffer
+class Verifier FLATBUFFERS_FINAL_CLASS {
+ public:
+  Verifier(const uint8_t *buf, size_t buf_len, uoffset_t _max_depth = 64,
+           uoffset_t _max_tables = 1000000, bool _check_alignment = true)
+      : buf_(buf),
+        size_(buf_len),
+        depth_(0),
+        max_depth_(_max_depth),
+        num_tables_(0),
+        max_tables_(_max_tables),
+        upper_bound_(0),
+        check_alignment_(_check_alignment) {
+    FLATBUFFERS_ASSERT(size_ < FLATBUFFERS_MAX_BUFFER_SIZE);
+  }
+
+  // Central location where any verification failures register.
+  bool Check(bool ok) const {
+    // clang-format off
+    #ifdef FLATBUFFERS_DEBUG_VERIFICATION_FAILURE
+      FLATBUFFERS_ASSERT(ok);
+    #endif
+    #ifdef FLATBUFFERS_TRACK_VERIFIER_BUFFER_SIZE
+      if (!ok)
+        upper_bound_ = 0;
+    #endif
+    // clang-format on
+    return ok;
+  }
+
+  // Verify any range within the buffer.
+  bool Verify(size_t elem, size_t elem_len) const {
+    // clang-format off
+    #ifdef FLATBUFFERS_TRACK_VERIFIER_BUFFER_SIZE
+      auto upper_bound = elem + elem_len;
+      if (upper_bound_ < upper_bound)
+        upper_bound_ =  upper_bound;
+    #endif
+    // clang-format on
+    return Check(elem_len < size_ && elem <= size_ - elem_len);
+  }
+
+  template<typename T> bool VerifyAlignment(size_t elem) const {
+    return (elem & (sizeof(T) - 1)) == 0 || !check_alignment_;
+  }
+
+  // Verify a range indicated by sizeof(T).
+  template<typename T> bool Verify(size_t elem) const {
+    return VerifyAlignment<T>(elem) && Verify(elem, sizeof(T));
+  }
+
+  bool VerifyFromPointer(const uint8_t *p, size_t len) {
+    auto o = static_cast<size_t>(p - buf_);
+    return Verify(o, len);
+  }
+
+  // Verify relative to a known-good base pointer.
+  bool Verify(const uint8_t *base, voffset_t elem_off, size_t elem_len) const {
+    return Verify(static_cast<size_t>(base - buf_) + elem_off, elem_len);
+  }
+
+  template<typename T>
+  bool Verify(const uint8_t *base, voffset_t elem_off) const {
+    return Verify(static_cast<size_t>(base - buf_) + elem_off, sizeof(T));
+  }
+
+  // Verify a pointer (may be NULL) of a table type.
+  template<typename T> bool VerifyTable(const T *table) {
+    return !table || table->Verify(*this);
+  }
+
+  // Verify a pointer (may be NULL) of any vector type.
+  template<typename T> bool VerifyVector(const Vector<T> *vec) const {
+    return !vec || VerifyVectorOrString(reinterpret_cast<const uint8_t *>(vec),
+                                        sizeof(T));
+  }
+
+  // Verify a pointer (may be NULL) of a vector to struct.
+  template<typename T> bool VerifyVector(const Vector<const T *> *vec) const {
+    return VerifyVector(reinterpret_cast<const Vector<T> *>(vec));
+  }
+
+  // Verify a pointer (may be NULL) to string.
+  bool VerifyString(const String *str) const {
+    size_t end;
+    return !str || (VerifyVectorOrString(reinterpret_cast<const uint8_t *>(str),
+                                         1, &end) &&
+                    Verify(end, 1) &&           // Must have terminator
+                    Check(buf_[end] == '\0'));  // Terminating byte must be 0.
+  }
+
+  // Common code between vectors and strings.
+  bool VerifyVectorOrString(const uint8_t *vec, size_t elem_size,
+                            size_t *end = nullptr) const {
+    auto veco = static_cast<size_t>(vec - buf_);
+    // Check we can read the size field.
+    if (!Verify<uoffset_t>(veco)) return false;
+    // Check the whole array. If this is a string, the byte past the array
+    // must be 0.
+    auto size = ReadScalar<uoffset_t>(vec);
+    auto max_elems = FLATBUFFERS_MAX_BUFFER_SIZE / elem_size;
+    if (!Check(size < max_elems))
+      return false;  // Protect against byte_size overflowing.
+    auto byte_size = sizeof(size) + elem_size * size;
+    if (end) *end = veco + byte_size;
+    return Verify(veco, byte_size);
+  }
+
+  // Special case for string contents, after the above has been called.
+  bool VerifyVectorOfStrings(const Vector<Offset<String>> *vec) const {
+    if (vec) {
+      for (uoffset_t i = 0; i < vec->size(); i++) {
+        if (!VerifyString(vec->Get(i))) return false;
+      }
+    }
+    return true;
+  }
+
+  // Special case for table contents, after the above has been called.
+  template<typename T> bool VerifyVectorOfTables(const Vector<Offset<T>> *vec) {
+    if (vec) {
+      for (uoffset_t i = 0; i < vec->size(); i++) {
+        if (!vec->Get(i)->Verify(*this)) return false;
+      }
+    }
+    return true;
+  }
+
+  __supress_ubsan__("unsigned-integer-overflow") bool VerifyTableStart(
+      const uint8_t *table) {
+    // Check the vtable offset.
+    auto tableo = static_cast<size_t>(table - buf_);
+    if (!Verify<soffset_t>(tableo)) return false;
+    // This offset may be signed, but doing the subtraction unsigned always
+    // gives the result we want.
+    auto vtableo = tableo - static_cast<size_t>(ReadScalar<soffset_t>(table));
+    // Check the vtable size field, then check vtable fits in its entirety.
+    return VerifyComplexity() && Verify<voffset_t>(vtableo) &&
+           VerifyAlignment<voffset_t>(ReadScalar<voffset_t>(buf_ + vtableo)) &&
+           Verify(vtableo, ReadScalar<voffset_t>(buf_ + vtableo));
+  }
+
+  template<typename T>
+  bool VerifyBufferFromStart(const char *identifier, size_t start) {
+    if (identifier && (size_ < 2 * sizeof(flatbuffers::uoffset_t) ||
+                       !BufferHasIdentifier(buf_ + start, identifier))) {
+      return false;
+    }
+
+    // Call T::Verify, which must be in the generated code for this type.
+    auto o = VerifyOffset(start);
+    return o && reinterpret_cast<const T *>(buf_ + start + o)->Verify(*this)
+    // clang-format off
+    #ifdef FLATBUFFERS_TRACK_VERIFIER_BUFFER_SIZE
+           && GetComputedSize()
+    #endif
+        ;
+    // clang-format on
+  }
+
+  // Verify this whole buffer, starting with root type T.
+  template<typename T> bool VerifyBuffer() { return VerifyBuffer<T>(nullptr); }
+
+  template<typename T> bool VerifyBuffer(const char *identifier) {
+    return VerifyBufferFromStart<T>(identifier, 0);
+  }
+
+  template<typename T> bool VerifySizePrefixedBuffer(const char *identifier) {
+    return Verify<uoffset_t>(0U) &&
+           ReadScalar<uoffset_t>(buf_) == size_ - sizeof(uoffset_t) &&
+           VerifyBufferFromStart<T>(identifier, sizeof(uoffset_t));
+  }
+
+  uoffset_t VerifyOffset(size_t start) const {
+    if (!Verify<uoffset_t>(start)) return 0;
+    auto o = ReadScalar<uoffset_t>(buf_ + start);
+    // May not point to itself.
+    if (!Check(o != 0)) return 0;
+    // Can't wrap around / buffers are max 2GB.
+    if (!Check(static_cast<soffset_t>(o) >= 0)) return 0;
+    // Must be inside the buffer to create a pointer from it (pointer outside
+    // buffer is UB).
+    if (!Verify(start + o, 1)) return 0;
+    return o;
+  }
+
+  uoffset_t VerifyOffset(const uint8_t *base, voffset_t start) const {
+    return VerifyOffset(static_cast<size_t>(base - buf_) + start);
+  }
+
+  // Called at the start of a table to increase counters measuring data
+  // structure depth and amount, and possibly bails out with false if
+  // limits set by the constructor have been hit. Needs to be balanced
+  // with EndTable().
+  bool VerifyComplexity() {
+    depth_++;
+    num_tables_++;
+    return Check(depth_ <= max_depth_ && num_tables_ <= max_tables_);
+  }
+
+  // Called at the end of a table to pop the depth count.
+  bool EndTable() {
+    depth_--;
+    return true;
+  }
+
+  // Returns the message size in bytes
+  size_t GetComputedSize() const {
+    // clang-format off
+    #ifdef FLATBUFFERS_TRACK_VERIFIER_BUFFER_SIZE
+      uintptr_t size = upper_bound_;
+      // Align the size to uoffset_t
+      size = (size - 1 + sizeof(uoffset_t)) & ~(sizeof(uoffset_t) - 1);
+      return (size > size_) ?  0 : size;
+    #else
+      // Must turn on FLATBUFFERS_TRACK_VERIFIER_BUFFER_SIZE for this to work.
+      (void)upper_bound_;
+      FLATBUFFERS_ASSERT(false);
+      return 0;
+    #endif
+    // clang-format on
+  }
+
+ private:
+  const uint8_t *buf_;
+  size_t size_;
+  uoffset_t depth_;
+  uoffset_t max_depth_;
+  uoffset_t num_tables_;
+  uoffset_t max_tables_;
+  mutable size_t upper_bound_;
+  bool check_alignment_;
+};
+
+// Convenient way to bundle a buffer and its length, to pass it around
+// typed by its root.
+// A BufferRef does not own its buffer.
+struct BufferRefBase {};  // for std::is_base_of
+template<typename T> struct BufferRef : BufferRefBase {
+  BufferRef() : buf(nullptr), len(0), must_free(false) {}
+  BufferRef(uint8_t *_buf, uoffset_t _len)
+      : buf(_buf), len(_len), must_free(false) {}
+
+  ~BufferRef() {
+    if (must_free) free(buf);
+  }
+
+  const T *GetRoot() const { return flatbuffers::GetRoot<T>(buf); }
+
+  bool Verify() {
+    Verifier verifier(buf, len);
+    return verifier.VerifyBuffer<T>(nullptr);
+  }
+
+  uint8_t *buf;
+  uoffset_t len;
+  bool must_free;
+};
+
+// "structs" are flat structures that do not have an offset table, thus
+// always have all members present and do not support forwards/backwards
+// compatible extensions.
+
+class Struct FLATBUFFERS_FINAL_CLASS {
+ public:
+  template<typename T> T GetField(uoffset_t o) const {
+    return ReadScalar<T>(&data_[o]);
+  }
+
+  template<typename T> T GetStruct(uoffset_t o) const {
+    return reinterpret_cast<T>(&data_[o]);
+  }
+
+  const uint8_t *GetAddressOf(uoffset_t o) const { return &data_[o]; }
+  uint8_t *GetAddressOf(uoffset_t o) { return &data_[o]; }
+
+ private:
+  uint8_t data_[1];
+};
+
+// "tables" use an offset table (possibly shared) that allows fields to be
+// omitted and added at will, but uses an extra indirection to read.
+class Table {
+ public:
+  const uint8_t *GetVTable() const {
+    return data_ - ReadScalar<soffset_t>(data_);
+  }
+
+  // This gets the field offset for any of the functions below it, or 0
+  // if the field was not present.
+  voffset_t GetOptionalFieldOffset(voffset_t field) const {
+    // The vtable offset is always at the start.
+    auto vtable = GetVTable();
+    // The first element is the size of the vtable (fields + type id + itself).
+    auto vtsize = ReadScalar<voffset_t>(vtable);
+    // If the field we're accessing is outside the vtable, we're reading older
+    // data, so it's the same as if the offset was 0 (not present).
+    return field < vtsize ? ReadScalar<voffset_t>(vtable + field) : 0;
+  }
+
+  template<typename T> T GetField(voffset_t field, T defaultval) const {
+    auto field_offset = GetOptionalFieldOffset(field);
+    return field_offset ? ReadScalar<T>(data_ + field_offset) : defaultval;
+  }
+
+  template<typename P> P GetPointer(voffset_t field) {
+    auto field_offset = GetOptionalFieldOffset(field);
+    auto p = data_ + field_offset;
+    return field_offset ? reinterpret_cast<P>(p + ReadScalar<uoffset_t>(p))
+                        : nullptr;
+  }
+  template<typename P> P GetPointer(voffset_t field) const {
+    return const_cast<Table *>(this)->GetPointer<P>(field);
+  }
+
+  template<typename P> P GetStruct(voffset_t field) const {
+    auto field_offset = GetOptionalFieldOffset(field);
+    auto p = const_cast<uint8_t *>(data_ + field_offset);
+    return field_offset ? reinterpret_cast<P>(p) : nullptr;
+  }
+
+  template<typename T> bool SetField(voffset_t field, T val, T def) {
+    auto field_offset = GetOptionalFieldOffset(field);
+    if (!field_offset) return IsTheSameAs(val, def);
+    WriteScalar(data_ + field_offset, val);
+    return true;
+  }
+
+  bool SetPointer(voffset_t field, const uint8_t *val) {
+    auto field_offset = GetOptionalFieldOffset(field);
+    if (!field_offset) return false;
+    WriteScalar(data_ + field_offset,
+                static_cast<uoffset_t>(val - (data_ + field_offset)));
+    return true;
+  }
+
+  uint8_t *GetAddressOf(voffset_t field) {
+    auto field_offset = GetOptionalFieldOffset(field);
+    return field_offset ? data_ + field_offset : nullptr;
+  }
+  const uint8_t *GetAddressOf(voffset_t field) const {
+    return const_cast<Table *>(this)->GetAddressOf(field);
+  }
+
+  bool CheckField(voffset_t field) const {
+    return GetOptionalFieldOffset(field) != 0;
+  }
+
+  // Verify the vtable of this table.
+  // Call this once per table, followed by VerifyField once per field.
+  bool VerifyTableStart(Verifier &verifier) const {
+    return verifier.VerifyTableStart(data_);
+  }
+
+  // Verify a particular field.
+  template<typename T>
+  bool VerifyField(const Verifier &verifier, voffset_t field) const {
+    // Calling GetOptionalFieldOffset should be safe now thanks to
+    // VerifyTable().
+    auto field_offset = GetOptionalFieldOffset(field);
+    // Check the actual field.
+    return !field_offset || verifier.Verify<T>(data_, field_offset);
+  }
+
+  // VerifyField for required fields.
+  template<typename T>
+  bool VerifyFieldRequired(const Verifier &verifier, voffset_t field) const {
+    auto field_offset = GetOptionalFieldOffset(field);
+    return verifier.Check(field_offset != 0) &&
+           verifier.Verify<T>(data_, field_offset);
+  }
+
+  // Versions for offsets.
+  bool VerifyOffset(const Verifier &verifier, voffset_t field) const {
+    auto field_offset = GetOptionalFieldOffset(field);
+    return !field_offset || verifier.VerifyOffset(data_, field_offset);
+  }
+
+  bool VerifyOffsetRequired(const Verifier &verifier, voffset_t field) const {
+    auto field_offset = GetOptionalFieldOffset(field);
+    return verifier.Check(field_offset != 0) &&
+           verifier.VerifyOffset(data_, field_offset);
+  }
+
+ private:
+  // private constructor & copy constructor: you obtain instances of this
+  // class by pointing to existing data only
+  Table();
+  Table(const Table &other);
+
+  uint8_t data_[1];
+};
+
+template<typename T>
+void FlatBufferBuilder::Required(Offset<T> table, voffset_t field) {
+  auto table_ptr = reinterpret_cast<const Table *>(buf_.data_at(table.o));
+  bool ok = table_ptr->GetOptionalFieldOffset(field) != 0;
+  // If this fails, the caller will show what field needs to be set.
+  FLATBUFFERS_ASSERT(ok);
+  (void)ok;
+}
+
+/// @brief This can compute the start of a FlatBuffer from a root pointer, i.e.
+/// it is the opposite transformation of GetRoot().
+/// This may be useful if you want to pass on a root and have the recipient
+/// delete the buffer afterwards.
+inline const uint8_t *GetBufferStartFromRootPointer(const void *root) {
+  auto table = reinterpret_cast<const Table *>(root);
+  auto vtable = table->GetVTable();
+  // Either the vtable is before the root or after the root.
+  auto start = (std::min)(vtable, reinterpret_cast<const uint8_t *>(root));
+  // Align to at least sizeof(uoffset_t).
+  start = reinterpret_cast<const uint8_t *>(reinterpret_cast<uintptr_t>(start) &
+                                            ~(sizeof(uoffset_t) - 1));
+  // Additionally, there may be a file_identifier in the buffer, and the root
+  // offset. The buffer may have been aligned to any size between
+  // sizeof(uoffset_t) and FLATBUFFERS_MAX_ALIGNMENT (see "force_align").
+  // Sadly, the exact alignment is only known when constructing the buffer,
+  // since it depends on the presence of values with said alignment properties.
+  // So instead, we simply look at the next uoffset_t values (root,
+  // file_identifier, and alignment padding) to see which points to the root.
+  // None of the other values can "impersonate" the root since they will either
+  // be 0 or four ASCII characters.
+  static_assert(FlatBufferBuilder::kFileIdentifierLength == sizeof(uoffset_t),
+                "file_identifier is assumed to be the same size as uoffset_t");
+  for (auto possible_roots = FLATBUFFERS_MAX_ALIGNMENT / sizeof(uoffset_t) + 1;
+       possible_roots; possible_roots--) {
+    start -= sizeof(uoffset_t);
+    if (ReadScalar<uoffset_t>(start) + start ==
+        reinterpret_cast<const uint8_t *>(root))
+      return start;
+  }
+  // We didn't find the root, either the "root" passed isn't really a root,
+  // or the buffer is corrupt.
+  // Assert, because calling this function with bad data may cause reads
+  // outside of buffer boundaries.
+  FLATBUFFERS_ASSERT(false);
+  return nullptr;
+}
+
+/// @brief This return the prefixed size of a FlatBuffer.
+inline uoffset_t GetPrefixedSize(const uint8_t *buf) {
+  return ReadScalar<uoffset_t>(buf);
+}
+
+// Base class for native objects (FlatBuffer data de-serialized into native
+// C++ data structures).
+// Contains no functionality, purely documentative.
+struct NativeTable {};
+
+/// @brief Function types to be used with resolving hashes into objects and
+/// back again. The resolver gets a pointer to a field inside an object API
+/// object that is of the type specified in the schema using the attribute
+/// `cpp_type` (it is thus important whatever you write to this address
+/// matches that type). The value of this field is initially null, so you
+/// may choose to implement a delayed binding lookup using this function
+/// if you wish. The resolver does the opposite lookup, for when the object
+/// is being serialized again.
+typedef uint64_t hash_value_t;
+// clang-format off
+#ifdef FLATBUFFERS_CPP98_STL
+  typedef void (*resolver_function_t)(void **pointer_adr, hash_value_t hash);
+  typedef hash_value_t (*rehasher_function_t)(void *pointer);
+#else
+  typedef std::function<void (void **pointer_adr, hash_value_t hash)>
+          resolver_function_t;
+  typedef std::function<hash_value_t (void *pointer)> rehasher_function_t;
+#endif
+// clang-format on
+
+// Helper function to test if a field is present, using any of the field
+// enums in the generated code.
+// `table` must be a generated table type. Since this is a template parameter,
+// this is not typechecked to be a subclass of Table, so beware!
+// Note: this function will return false for fields equal to the default
+// value, since they're not stored in the buffer (unless force_defaults was
+// used).
+template<typename T>
+bool IsFieldPresent(const T *table, typename T::FlatBuffersVTableOffset field) {
+  // Cast, since Table is a private baseclass of any table types.
+  return reinterpret_cast<const Table *>(table)->CheckField(
+      static_cast<voffset_t>(field));
+}
+
+// Utility function for reverse lookups on the EnumNames*() functions
+// (in the generated C++ code)
+// names must be NULL terminated.
+inline int LookupEnum(const char **names, const char *name) {
+  for (const char **p = names; *p; p++)
+    if (!strcmp(*p, name)) return static_cast<int>(p - names);
+  return -1;
+}
+
+// These macros allow us to layout a struct with a guarantee that they'll end
+// up looking the same on different compilers and platforms.
+// It does this by disallowing the compiler to do any padding, and then
+// does padding itself by inserting extra padding fields that make every
+// element aligned to its own size.
+// Additionally, it manually sets the alignment of the struct as a whole,
+// which is typically its largest element, or a custom size set in the schema
+// by the force_align attribute.
+// These are used in the generated code only.
+
+// clang-format off
+#if defined(_MSC_VER)
+  #define FLATBUFFERS_MANUALLY_ALIGNED_STRUCT(alignment) \
+    __pragma(pack(1)) \
+    struct __declspec(align(alignment))
+  #define FLATBUFFERS_STRUCT_END(name, size) \
+    __pragma(pack()) \
+    static_assert(sizeof(name) == size, "compiler breaks packing rules")
+#elif defined(__GNUC__) || defined(__clang__) || defined(__ICCARM__)
+  #define FLATBUFFERS_MANUALLY_ALIGNED_STRUCT(alignment) \
+    _Pragma("pack(1)") \
+    struct __attribute__((aligned(alignment)))
+  #define FLATBUFFERS_STRUCT_END(name, size) \
+    _Pragma("pack()") \
+    static_assert(sizeof(name) == size, "compiler breaks packing rules")
+#else
+  #error Unknown compiler, please define structure alignment macros
+#endif
+// clang-format on
+
+// Minimal reflection via code generation.
+// Besides full-fat reflection (see reflection.h) and parsing/printing by
+// loading schemas (see idl.h), we can also have code generation for mimimal
+// reflection data which allows pretty-printing and other uses without needing
+// a schema or a parser.
+// Generate code with --reflect-types (types only) or --reflect-names (names
+// also) to enable.
+// See minireflect.h for utilities using this functionality.
+
+// These types are organized slightly differently as the ones in idl.h.
+enum SequenceType { ST_TABLE, ST_STRUCT, ST_UNION, ST_ENUM };
+
+// Scalars have the same order as in idl.h
+// clang-format off
+#define FLATBUFFERS_GEN_ELEMENTARY_TYPES(ET) \
+  ET(ET_UTYPE) \
+  ET(ET_BOOL) \
+  ET(ET_CHAR) \
+  ET(ET_UCHAR) \
+  ET(ET_SHORT) \
+  ET(ET_USHORT) \
+  ET(ET_INT) \
+  ET(ET_UINT) \
+  ET(ET_LONG) \
+  ET(ET_ULONG) \
+  ET(ET_FLOAT) \
+  ET(ET_DOUBLE) \
+  ET(ET_STRING) \
+  ET(ET_SEQUENCE)  // See SequenceType.
+
+enum ElementaryType {
+  #define FLATBUFFERS_ET(E) E,
+    FLATBUFFERS_GEN_ELEMENTARY_TYPES(FLATBUFFERS_ET)
+  #undef FLATBUFFERS_ET
+};
+
+inline const char * const *ElementaryTypeNames() {
+  static const char * const names[] = {
+    #define FLATBUFFERS_ET(E) #E,
+      FLATBUFFERS_GEN_ELEMENTARY_TYPES(FLATBUFFERS_ET)
+    #undef FLATBUFFERS_ET
+  };
+  return names;
+}
+// clang-format on
+
+// Basic type info cost just 16bits per field!
+struct TypeCode {
+  uint16_t base_type : 4;  // ElementaryType
+  uint16_t is_vector : 1;
+  int16_t sequence_ref : 11;  // Index into type_refs below, or -1 for none.
+};
+
+static_assert(sizeof(TypeCode) == 2, "TypeCode");
+
+struct TypeTable;
+
+// Signature of the static method present in each type.
+typedef const TypeTable *(*TypeFunction)();
+
+struct TypeTable {
+  SequenceType st;
+  size_t num_elems;  // of type_codes, values, names (but not type_refs).
+  const TypeCode *type_codes;     // num_elems count
+  const TypeFunction *type_refs;  // less than num_elems entries (see TypeCode).
+  const int64_t *values;  // Only set for non-consecutive enum/union or structs.
+  const char *const *names;  // Only set if compiled with --reflect-names.
+};
+
+// String which identifies the current version of FlatBuffers.
+// flatbuffer_version_string is used by Google developers to identify which
+// applications uploaded to Google Play are using this library.  This allows
+// the development team at Google to determine the popularity of the library.
+// How it works: Applications that are uploaded to the Google Play Store are
+// scanned for this version string.  We track which applications are using it
+// to measure popularity.  You are free to remove it (of course) but we would
+// appreciate if you left it in.
+
+// Weak linkage is culled by VS & doesn't work on cygwin.
+// clang-format off
+#if !defined(_WIN32) && !defined(__CYGWIN__)
+
+extern volatile __attribute__((weak)) const char *flatbuffer_version_string;
+volatile __attribute__((weak)) const char *flatbuffer_version_string =
+  "FlatBuffers "
+  FLATBUFFERS_STRING(FLATBUFFERS_VERSION_MAJOR) "."
+  FLATBUFFERS_STRING(FLATBUFFERS_VERSION_MINOR) "."
+  FLATBUFFERS_STRING(FLATBUFFERS_VERSION_REVISION);
+
+#endif  // !defined(_WIN32) && !defined(__CYGWIN__)
+
+#define FLATBUFFERS_DEFINE_BITMASK_OPERATORS(E, T)\
+    inline E operator | (E lhs, E rhs){\
+        return E(T(lhs) | T(rhs));\
+    }\
+    inline E operator & (E lhs, E rhs){\
+        return E(T(lhs) & T(rhs));\
+    }\
+    inline E operator ^ (E lhs, E rhs){\
+        return E(T(lhs) ^ T(rhs));\
+    }\
+    inline E operator ~ (E lhs){\
+        return E(~T(lhs));\
+    }\
+    inline E operator |= (E &lhs, E rhs){\
+        lhs = lhs | rhs;\
+        return lhs;\
+    }\
+    inline E operator &= (E &lhs, E rhs){\
+        lhs = lhs & rhs;\
+        return lhs;\
+    }\
+    inline E operator ^= (E &lhs, E rhs){\
+        lhs = lhs ^ rhs;\
+        return lhs;\
+    }\
+    inline bool operator !(E rhs) \
+    {\
+        return !bool(T(rhs)); \
+    }
+/// @endcond
+}  // namespace flatbuffers
+
+// clang-format on
+
+#endif  // FLATBUFFERS_H_
diff --git a/include/behaviortree_cpp_v3/flatbuffers/stl_emulation.h b/include/behaviortree_cpp_v3/flatbuffers/stl_emulation.h
new file mode 100644 (file)
index 0000000..e68089f
--- /dev/null
@@ -0,0 +1,287 @@
+/*
+ * Copyright 2017 Google Inc. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef FLATBUFFERS_STL_EMULATION_H_
+#define FLATBUFFERS_STL_EMULATION_H_
+
+// clang-format off
+
+#include <string>
+#include <type_traits>
+#include <vector>
+#include <memory>
+#include <limits>
+
+#if defined(_STLPORT_VERSION) && !defined(FLATBUFFERS_CPP98_STL)
+  #define FLATBUFFERS_CPP98_STL
+#endif  // defined(_STLPORT_VERSION) && !defined(FLATBUFFERS_CPP98_STL)
+
+#if defined(FLATBUFFERS_CPP98_STL)
+  #include <cctype>
+#endif  // defined(FLATBUFFERS_CPP98_STL)
+
+// Check if we can use template aliases
+// Not possible if Microsoft Compiler before 2012
+// Possible is the language feature __cpp_alias_templates is defined well
+// Or possible if the C++ std is C+11 or newer
+#if (defined(_MSC_VER) && _MSC_VER > 1700 /* MSVC2012 */) \
+    || (defined(__cpp_alias_templates) && __cpp_alias_templates >= 200704) \
+    || (defined(__cplusplus) && __cplusplus >= 201103L)
+  #define FLATBUFFERS_TEMPLATES_ALIASES
+#endif
+
+// This header provides backwards compatibility for C++98 STLs like stlport.
+namespace flatbuffers {
+
+// Retrieve ::back() from a string in a way that is compatible with pre C++11
+// STLs (e.g stlport).
+inline char& string_back(std::string &value) {
+  return value[value.length() - 1];
+}
+
+inline char string_back(const std::string &value) {
+  return value[value.length() - 1];
+}
+
+// Helper method that retrieves ::data() from a vector in a way that is
+// compatible with pre C++11 STLs (e.g stlport).
+template <typename T> inline T *vector_data(std::vector<T> &vector) {
+  // In some debug environments, operator[] does bounds checking, so &vector[0]
+  // can't be used.
+  return vector.empty() ? nullptr : &vector[0];
+}
+
+template <typename T> inline const T *vector_data(
+    const std::vector<T> &vector) {
+  return vector.empty() ? nullptr : &vector[0];
+}
+
+template <typename T, typename V>
+inline void vector_emplace_back(std::vector<T> *vector, V &&data) {
+  #if defined(FLATBUFFERS_CPP98_STL)
+    vector->push_back(data);
+  #else
+    vector->emplace_back(std::forward<V>(data));
+  #endif  // defined(FLATBUFFERS_CPP98_STL)
+}
+
+#ifndef FLATBUFFERS_CPP98_STL
+  #if defined(FLATBUFFERS_TEMPLATES_ALIASES)
+    template <typename T>
+    using numeric_limits = std::numeric_limits<T>;
+  #else
+    template <typename T> class numeric_limits :
+      public std::numeric_limits<T> {};
+  #endif  // defined(FLATBUFFERS_TEMPLATES_ALIASES)
+#else
+  template <typename T> class numeric_limits :
+      public std::numeric_limits<T> {
+    public:
+      // Android NDK fix.
+      static T lowest() {
+        return std::numeric_limits<T>::min();
+      }
+  };
+
+  template <> class numeric_limits<float> : 
+      public std::numeric_limits<float> {
+    public:
+      static float lowest() { return -FLT_MAX; }
+  };
+
+  template <> class numeric_limits<double> : 
+      public std::numeric_limits<double> {
+    public:
+      static double lowest() { return -DBL_MAX; }
+  };
+
+  template <> class numeric_limits<unsigned long long> {
+   public:
+    static unsigned long long min() { return 0ULL; }
+    static unsigned long long max() { return ~0ULL; }
+    static unsigned long long lowest() {
+      return numeric_limits<unsigned long long>::min();
+    }
+  };
+
+  template <> class numeric_limits<long long> {
+   public:
+    static long long min() {
+      return static_cast<long long>(1ULL << ((sizeof(long long) << 3) - 1));
+    }
+    static long long max() {
+      return static_cast<long long>(
+          (1ULL << ((sizeof(long long) << 3) - 1)) - 1);
+    }
+    static long long lowest() {
+      return numeric_limits<long long>::min();
+    }
+  };
+#endif  // FLATBUFFERS_CPP98_STL
+
+#if defined(FLATBUFFERS_TEMPLATES_ALIASES)
+  #ifndef FLATBUFFERS_CPP98_STL
+    template <typename T> using is_scalar = std::is_scalar<T>;
+    template <typename T, typename U> using is_same = std::is_same<T,U>;
+    template <typename T> using is_floating_point = std::is_floating_point<T>;
+    template <typename T> using is_unsigned = std::is_unsigned<T>;
+    template <typename T> using make_unsigned = std::make_unsigned<T>;
+    template<bool B, class T, class F>
+    using conditional = std::conditional<B, T, F>;
+    template<class T, T v>
+    using integral_constant = std::integral_constant<T, v>;
+#else
+    // Map C++ TR1 templates defined by stlport.
+    template <typename T> using is_scalar = std::tr1::is_scalar<T>;
+    template <typename T, typename U> using is_same = std::tr1::is_same<T,U>;
+    template <typename T> using is_floating_point =
+        std::tr1::is_floating_point<T>;
+    template <typename T> using is_unsigned = std::tr1::is_unsigned<T>;
+    // Android NDK doesn't have std::make_unsigned or std::tr1::make_unsigned.
+    template<typename T> struct make_unsigned {
+      static_assert(is_unsigned<T>::value, "Specialization not implemented!");
+      using type = T;
+    };
+    template<> struct make_unsigned<char> { using type = unsigned char; };
+    template<> struct make_unsigned<short> { using type = unsigned short; };
+    template<> struct make_unsigned<int> { using type = unsigned int; };
+    template<> struct make_unsigned<long> { using type = unsigned long; };
+    template<>
+    struct make_unsigned<long long> { using type = unsigned long long; };
+    template<bool B, class T, class F>
+    using conditional = std::tr1::conditional<B, T, F>;
+    template<class T, T v>
+    using integral_constant = std::tr1::integral_constant<T, v>;
+#endif  // !FLATBUFFERS_CPP98_STL
+#else
+  // MSVC 2010 doesn't support C++11 aliases.
+  template <typename T> struct is_scalar : public std::is_scalar<T> {};
+  template <typename T, typename U> struct is_same : public std::is_same<T,U> {};
+  template <typename T> struct is_floating_point :
+        public std::is_floating_point<T> {};
+  template <typename T> struct is_unsigned : public std::is_unsigned<T> {};
+  template <typename T> struct make_unsigned : public std::make_unsigned<T> {};
+  template<bool B, class T, class F>
+  struct conditional : public std::conditional<B, T, F> {};
+  template<class T, T v>
+  struct integral_constant : public std::integral_constant<T, v> {};
+#endif  // defined(FLATBUFFERS_TEMPLATES_ALIASES)
+
+#ifndef FLATBUFFERS_CPP98_STL
+  #if defined(FLATBUFFERS_TEMPLATES_ALIASES)
+    template <class T> using unique_ptr = std::unique_ptr<T>;
+  #else
+    // MSVC 2010 doesn't support C++11 aliases.
+    // We're manually "aliasing" the class here as we want to bring unique_ptr
+    // into the flatbuffers namespace.  We have unique_ptr in the flatbuffers
+    // namespace we have a completely independent implemenation (see below)
+    // for C++98 STL implementations.
+    template <class T> class unique_ptr : public std::unique_ptr<T> {
+     public:
+      unique_ptr() {}
+      explicit unique_ptr(T* p) : std::unique_ptr<T>(p) {}
+      unique_ptr(std::unique_ptr<T>&& u) { *this = std::move(u); }
+      unique_ptr(unique_ptr&& u) { *this = std::move(u); }
+      unique_ptr& operator=(std::unique_ptr<T>&& u) {
+        std::unique_ptr<T>::reset(u.release());
+        return *this;
+      }
+      unique_ptr& operator=(unique_ptr&& u) {
+        std::unique_ptr<T>::reset(u.release());
+        return *this;
+      }
+      unique_ptr& operator=(T* p) {
+        return std::unique_ptr<T>::operator=(p);
+      }
+    };
+  #endif  // defined(FLATBUFFERS_TEMPLATES_ALIASES)
+#else
+  // Very limited implementation of unique_ptr.
+  // This is provided simply to allow the C++ code generated from the default
+  // settings to function in C++98 environments with no modifications.
+  template <class T> class unique_ptr {
+   public:
+    typedef T element_type;
+
+    unique_ptr() : ptr_(nullptr) {}
+    explicit unique_ptr(T* p) : ptr_(p) {}
+    unique_ptr(unique_ptr&& u) : ptr_(nullptr) { reset(u.release()); }
+    unique_ptr(const unique_ptr& u) : ptr_(nullptr) {
+      reset(const_cast<unique_ptr*>(&u)->release());
+    }
+    ~unique_ptr() { reset(); }
+
+    unique_ptr& operator=(const unique_ptr& u) {
+      reset(const_cast<unique_ptr*>(&u)->release());
+      return *this;
+    }
+
+    unique_ptr& operator=(unique_ptr&& u) {
+      reset(u.release());
+      return *this;
+    }
+
+    unique_ptr& operator=(T* p) {
+      reset(p);
+      return *this;
+    }
+
+    const T& operator*() const { return *ptr_; }
+    T* operator->() const { return ptr_; }
+    T* get() const noexcept { return ptr_; }
+    explicit operator bool() const { return ptr_ != nullptr; }
+
+    // modifiers
+    T* release() {
+      T* value = ptr_;
+      ptr_ = nullptr;
+      return value;
+    }
+
+    void reset(T* p = nullptr) {
+      T* value = ptr_;
+      ptr_ = p;
+      if (value) delete value;
+    }
+
+    void swap(unique_ptr& u) {
+      T* temp_ptr = ptr_;
+      ptr_ = u.ptr_;
+      u.ptr_ = temp_ptr;
+    }
+
+   private:
+    T* ptr_;
+  };
+
+  template <class T> bool operator==(const unique_ptr<T>& x,
+                                     const unique_ptr<T>& y) {
+    return x.get() == y.get();
+  }
+
+  template <class T, class D> bool operator==(const unique_ptr<T>& x,
+                                              const D* y) {
+    return static_cast<D*>(x.get()) == y;
+  }
+
+  template <class T> bool operator==(const unique_ptr<T>& x, intptr_t y) {
+    return reinterpret_cast<intptr_t>(x.get()) == y;
+  }
+#endif  // !FLATBUFFERS_CPP98_STL
+
+}  // namespace flatbuffers
+
+#endif  // FLATBUFFERS_STL_EMULATION_H_
diff --git a/include/behaviortree_cpp_v3/leaf_node.h b/include/behaviortree_cpp_v3/leaf_node.h
new file mode 100644 (file)
index 0000000..e91a975
--- /dev/null
@@ -0,0 +1,33 @@
+/* Copyright (C) 2015-2018 Michele Colledanchise -  All Rights Reserved
+ * Copyright (C) 2018-2020 Davide Faconti, Eurecat -  All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#ifndef LEAFNODE_H
+#define LEAFNODE_H
+
+#include "behaviortree_cpp_v3/tree_node.h"
+
+namespace BT
+{
+class LeafNode : public TreeNode
+{
+  protected:
+  public:
+    LeafNode(const std::string& name, const NodeConfiguration& config)
+      : TreeNode(name, config)
+    { }
+
+    virtual ~LeafNode() override = default;
+};
+}
+
+#endif
diff --git a/include/behaviortree_cpp_v3/loggers/abstract_logger.h b/include/behaviortree_cpp_v3/loggers/abstract_logger.h
new file mode 100644 (file)
index 0000000..f8aaec8
--- /dev/null
@@ -0,0 +1,92 @@
+#ifndef ABSTRACT_LOGGER_H
+#define ABSTRACT_LOGGER_H
+
+#include "behaviortree_cpp_v3/behavior_tree.h"
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+namespace BT
+{
+enum class TimestampType
+{
+    ABSOLUTE,
+    RELATIVE
+};
+
+typedef std::array<uint8_t, 12> SerializedTransition;
+
+class StatusChangeLogger
+{
+  public:
+    StatusChangeLogger(TreeNode *root_node);
+    virtual ~StatusChangeLogger() = default;
+
+    virtual void callback(BT::Duration timestamp, const TreeNode& node, NodeStatus prev_status,
+                          NodeStatus status) = 0;
+
+    virtual void flush() = 0;
+
+    void setEnabled(bool enabled)
+    {
+        enabled_ = enabled;
+    }
+
+    void seTimestampType(TimestampType type)
+    {
+        type_ = type;
+    }
+
+    bool enabled() const
+    {
+        return enabled_;
+    }
+
+    // false by default.
+    bool showsTransitionToIdle() const
+    {
+        return show_transition_to_idle_;
+    }
+
+    void enableTransitionToIdle(bool enable)
+    {
+        show_transition_to_idle_ = enable;
+    }
+
+  private:
+    bool enabled_;
+    bool show_transition_to_idle_;
+    std::vector<TreeNode::StatusChangeSubscriber> subscribers_;
+    TimestampType type_;
+    BT::TimePoint first_timestamp_;
+};
+
+//--------------------------------------------
+
+inline StatusChangeLogger::StatusChangeLogger(TreeNode* root_node)
+  : enabled_(true), show_transition_to_idle_(true), type_(TimestampType::ABSOLUTE)
+{
+    first_timestamp_ = std::chrono::high_resolution_clock::now();
+
+    auto subscribeCallback = [this](TimePoint timestamp, const TreeNode& node, NodeStatus prev,
+                                    NodeStatus status) {
+        if (enabled_ && (status != NodeStatus::IDLE || show_transition_to_idle_))
+        {
+            if (type_ == TimestampType::ABSOLUTE)
+            {
+                this->callback(timestamp.time_since_epoch(), node, prev, status);
+            }
+            else
+            {
+                this->callback(timestamp - first_timestamp_, node, prev, status);
+            }
+        }
+    };
+
+    auto visitor = [this, subscribeCallback](TreeNode* node) {
+        subscribers_.push_back(node->subscribeToStatusChange(std::move(subscribeCallback)));
+    };
+
+    applyRecursiveVisitor(root_node, visitor);
+}
+}
+
+#endif   // ABSTRACT_LOGGER_H
diff --git a/include/behaviortree_cpp_v3/loggers/bt_cout_logger.h b/include/behaviortree_cpp_v3/loggers/bt_cout_logger.h
new file mode 100644 (file)
index 0000000..1b27f8a
--- /dev/null
@@ -0,0 +1,35 @@
+#ifndef BT_COUT_LOGGER_H
+#define BT_COUT_LOGGER_H
+
+#include <cstring>
+#include "abstract_logger.h"
+
+namespace BT
+{
+/**
+ * @brief AddStdCoutLoggerToTree. Give  the root node of a tree,
+ * a simple callback is subscribed to any status change of each node.
+ *
+ *
+ * @param root_node
+ * @return Important: the returned shared_ptr must not go out of scope,
+ *         otherwise the logger is removed.
+ */
+
+class StdCoutLogger : public StatusChangeLogger
+{
+    static std::atomic<bool> ref_count;
+
+  public:
+    StdCoutLogger(const BT::Tree& tree);
+    ~StdCoutLogger() override;
+
+    virtual void callback(Duration timestamp, const TreeNode& node, NodeStatus prev_status,
+                          NodeStatus status) override;
+
+    virtual void flush() override;
+};
+
+}   // end namespace
+
+#endif   // BT_COUT_LOGGER_H
diff --git a/include/behaviortree_cpp_v3/loggers/bt_file_logger.h b/include/behaviortree_cpp_v3/loggers/bt_file_logger.h
new file mode 100644 (file)
index 0000000..281d2ae
--- /dev/null
@@ -0,0 +1,35 @@
+#ifndef BT_FILE_LOGGER_H
+#define BT_FILE_LOGGER_H
+
+#include <fstream>
+#include <deque>
+#include <array>
+#include "abstract_logger.h"
+
+namespace BT
+{
+class FileLogger : public StatusChangeLogger
+{
+  public:
+    FileLogger(const Tree &tree, const char* filename, uint16_t buffer_size = 10);
+
+    virtual ~FileLogger() override;
+
+    virtual void callback(Duration timestamp, const TreeNode& node, NodeStatus prev_status,
+                          NodeStatus status) override;
+
+    virtual void flush() override;
+
+  private:
+    std::ofstream file_os_;
+
+    std::chrono::high_resolution_clock::time_point start_time;
+
+    std::vector<SerializedTransition> buffer_;
+
+    size_t buffer_max_size_;
+};
+
+}   // end namespace
+
+#endif   // BT_FILE_LOGGER_H
diff --git a/include/behaviortree_cpp_v3/loggers/bt_minitrace_logger.h b/include/behaviortree_cpp_v3/loggers/bt_minitrace_logger.h
new file mode 100644 (file)
index 0000000..80d7836
--- /dev/null
@@ -0,0 +1,29 @@
+#ifndef BT_MINITRACE_LOGGER_H
+#define BT_MINITRACE_LOGGER_H
+
+#include <cstring>
+#include "abstract_logger.h"
+
+namespace BT
+{
+class MinitraceLogger : public StatusChangeLogger
+{
+    static std::atomic<bool> ref_count;
+
+  public:
+    MinitraceLogger(const BT::Tree& tree, const char* filename_json);
+
+    virtual ~MinitraceLogger() override;
+
+    virtual void callback(Duration timestamp, const TreeNode& node, NodeStatus prev_status,
+                          NodeStatus status) override;
+
+    virtual void flush() override;
+
+  private:
+    TimePoint prev_time_;
+};
+
+}   // end namespace
+
+#endif   // BT_MINITRACE_LOGGER_H
diff --git a/include/behaviortree_cpp_v3/loggers/bt_zmq_publisher.h b/include/behaviortree_cpp_v3/loggers/bt_zmq_publisher.h
new file mode 100644 (file)
index 0000000..59bc259
--- /dev/null
@@ -0,0 +1,52 @@
+#ifndef BT_ZMQ_PUBLISHER_H
+#define BT_ZMQ_PUBLISHER_H
+
+#include <array>
+#include <future>
+#include "abstract_logger.h"
+
+
+namespace BT
+{
+class PublisherZMQ : public StatusChangeLogger
+{
+    static std::atomic<bool> ref_count;
+
+  public:
+    PublisherZMQ(const BT::Tree& tree,
+                 unsigned max_msg_per_second = 25,
+                 unsigned publisher_port = 1666,
+                 unsigned server_port = 1667);
+
+    virtual ~PublisherZMQ();
+
+  private:
+    virtual void callback(Duration timestamp, const TreeNode& node, NodeStatus prev_status,
+                          NodeStatus status) override;
+
+    virtual void flush() override;
+
+    const BT::Tree& tree_;
+    std::vector<uint8_t> tree_buffer_;
+    std::vector<uint8_t> status_buffer_;
+    std::vector<SerializedTransition> transition_buffer_;
+    std::chrono::microseconds min_time_between_msgs_;
+
+    std::atomic_bool active_server_;
+    std::thread thread_;
+
+    void createStatusBuffer();
+
+    TimePoint deadline_;
+    std::mutex mutex_;
+    std::atomic_bool send_pending_;
+
+    std::future<void> send_future_;
+
+    struct Pimpl;
+    Pimpl* zmq_;
+
+};
+}
+
+#endif   // BT_ZMQ_PUBLISHER_H
diff --git a/include/behaviortree_cpp_v3/tree_node.h b/include/behaviortree_cpp_v3/tree_node.h
new file mode 100644 (file)
index 0000000..aa39138
--- /dev/null
@@ -0,0 +1,294 @@
+/* Copyright (C) 2015-2018 Michele Colledanchise -  All Rights Reserved
+*  Copyright (C) 2018-2020 Davide Faconti, Eurecat -  All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#ifndef BEHAVIORTREECORE_TREENODE_H
+#define BEHAVIORTREECORE_TREENODE_H
+
+#include <condition_variable>
+#include <mutex>
+#include "behaviortree_cpp_v3/utils/signal.h"
+#include "behaviortree_cpp_v3/exceptions.h"
+#include "behaviortree_cpp_v3/basic_types.h"
+#include "behaviortree_cpp_v3/blackboard.h"
+#include "behaviortree_cpp_v3/utils/strcat.hpp"
+
+#ifdef _MSC_VER 
+#pragma warning(disable : 4127) 
+#endif
+
+namespace BT
+{
+/// This information is used mostly by the XMLParser.
+struct TreeNodeManifest
+{
+    NodeType type;
+    std::string registration_ID;
+    PortsList ports;
+};
+
+typedef std::unordered_map<std::string, std::string> PortsRemapping;
+
+struct NodeConfiguration
+{
+    NodeConfiguration()
+    {
+    }
+
+    Blackboard::Ptr blackboard;
+    PortsRemapping input_ports;
+    PortsRemapping output_ports;
+};
+
+/// Abstract base class for Behavior Tree Nodes
+class TreeNode
+{
+  public:
+    typedef std::shared_ptr<TreeNode> Ptr;
+
+    /**
+     * @brief TreeNode main constructor.
+     *
+     * @param name     name of the instance, not the type.
+     * @param config   information about input/output ports. See NodeConfiguration
+     *
+     * Note: If your custom node has ports, the derived class must implement:
+     *
+     *     static PortsList providedPorts();
+     */
+    TreeNode(std::string name, NodeConfiguration config);
+
+    virtual ~TreeNode() = default;
+
+    /// The method that should be used to invoke tick() and setStatus();
+    virtual BT::NodeStatus executeTick();
+
+    /// The method used to interrupt the execution of a RUNNING node.
+    /// Only Async nodes that may return RUNNING should implement it.
+    virtual void halt() = 0;
+
+    bool isHalted() const;
+
+    NodeStatus status() const;
+
+    /// Name of the instance, not the type
+    const std::string& name() const;
+
+    /// Blocking function that will sleep until the setStatus() is called with
+    /// either RUNNING, FAILURE or SUCCESS.
+    BT::NodeStatus waitValidStatus();
+
+    virtual NodeType type() const = 0;
+
+    using StatusChangeSignal = Signal<TimePoint, const TreeNode&, NodeStatus, NodeStatus>;
+    using StatusChangeSubscriber = StatusChangeSignal::Subscriber;
+    using StatusChangeCallback = StatusChangeSignal::CallableFunction;
+
+    /**
+     * @brief subscribeToStatusChange is used to attach a callback to a status change.
+     * When StatusChangeSubscriber goes out of scope (it is a shared_ptr) the callback
+     * is unsubscribed automatically.
+     *
+     * @param callback The callback to be execute when status change.
+     *
+     * @return the subscriber handle.
+     */
+    StatusChangeSubscriber subscribeToStatusChange(StatusChangeCallback callback);
+
+    // get an unique identifier of this instance of treeNode
+    uint16_t UID() const;
+
+    /// registrationName is the ID used by BehaviorTreeFactory to create an instance.
+    const std::string& registrationName() const;
+
+    /// Configuration passed at construction time. Can never change after the
+    /// creation of the TreeNode instance.
+    const NodeConfiguration& config() const;
+
+    /** Read an input port, which, in practice, is an entry in the blackboard.
+     * If the blackboard contains a std::string and T is not a string,
+     * convertFromString<T>() is used automatically to parse the text.
+     *
+     * @param key   the identifier (before remapping) of the port.
+     * @return      false if an error occurs.
+     */
+    template <typename T>
+    Result getInput(const std::string& key, T& destination) const;
+
+    /** Same as bool getInput(const std::string& key, T& destination)
+     * but using optional.
+     */
+    template <typename T>
+    Optional<T> getInput(const std::string& key) const
+    {
+        T out;
+        auto res = getInput(key, out);
+        return (res) ? Optional<T>(out) : nonstd::make_unexpected(res.error());
+    }
+
+    template <typename T>
+    Result setOutput(const std::string& key, const T& value);
+
+    // function provide mostrly for debugging purpose to see the raw value
+    // in the port (no remapping and no conversion to a type)
+    StringView getRawPortValue(const std::string &key) const;
+
+    /// Check a string and return true if it matches either one of these
+    /// two patterns:  {...} or ${...}
+    static bool isBlackboardPointer(StringView str);
+
+    static StringView stripBlackboardPointer(StringView str);
+
+    static Optional<StringView> getRemappedKey(StringView port_name, StringView remapping_value);
+
+  protected:
+    /// Method to be implemented by the user
+    virtual BT::NodeStatus tick() = 0;
+
+    friend class BehaviorTreeFactory;
+    friend class DecoratorNode;
+    friend class ControlNode;
+    friend class Tree;
+
+    // Only BehaviorTreeFactory should call this
+    void setRegistrationID(StringView ID)
+    {
+        registration_ID_.assign(ID.data(), ID.size());
+    }
+
+    void modifyPortsRemapping(const PortsRemapping& new_remapping);
+
+    void setStatus(NodeStatus new_status);
+
+  private:
+    const std::string name_;
+
+    NodeStatus status_;
+
+    std::condition_variable state_condition_variable_;
+
+    mutable std::mutex state_mutex_;
+
+    StatusChangeSignal state_change_signal_;
+
+    const uint16_t uid_;
+
+    NodeConfiguration config_;
+
+    std::string registration_ID_;
+};
+
+//-------------------------------------------------------
+template <typename T>
+inline Result TreeNode::getInput(const std::string& key, T& destination) const
+{
+    auto remap_it = config_.input_ports.find(key);
+    if (remap_it == config_.input_ports.end())
+    {
+        return nonstd::make_unexpected(StrCat("getInput() failed because "
+                                              "NodeConfiguration::input_ports "
+                                              "does not contain the key: [",
+                                              key, "]"));
+    }
+    auto remapped_res = getRemappedKey(key, remap_it->second);
+    try
+    {
+        if (!remapped_res)
+        {
+            destination = convertFromString<T>(remap_it->second);
+            return {};
+        }
+        const auto& remapped_key = remapped_res.value();
+
+        if (!config_.blackboard)
+        {
+            return nonstd::make_unexpected("getInput() trying to access a Blackboard(BB) entry, "
+                                           "but BB is invalid");
+        }
+
+        const Any* val = config_.blackboard->getAny(static_cast<std::string>(remapped_key));
+        if (val && val->empty() == false)
+        {
+            if (std::is_same<T, std::string>::value == false && val->type() == typeid(std::string))
+            {
+                destination = convertFromString<T>(val->cast<std::string>());
+            }
+            else
+            {
+                destination = val->cast<T>();
+            }
+            return {};
+        }
+
+        return nonstd::make_unexpected(StrCat("getInput() failed because it was unable to find the "
+                                              "key [",
+                                              key, "] remapped to [", remapped_key, "]"));
+    }
+    catch (std::exception& err)
+    {
+        return nonstd::make_unexpected(err.what());
+    }
+}
+
+template <typename T>
+inline Result TreeNode::setOutput(const std::string& key, const T& value)
+{
+    if (!config_.blackboard)
+    {
+        return nonstd::make_unexpected("setOutput() failed: trying to access a "
+                                       "Blackboard(BB) entry, but BB is invalid");
+    }
+
+    auto remap_it = config_.output_ports.find(key);
+    if (remap_it == config_.output_ports.end())
+    {
+        return nonstd::make_unexpected(StrCat("setOutput() failed: NodeConfiguration::output_ports "
+                                              "does not "
+                                              "contain the key: [",
+                                              key, "]"));
+    }
+    StringView remapped_key = remap_it->second;
+    if (remapped_key == "=")
+    {
+        remapped_key = key;
+    }
+    if (isBlackboardPointer(remapped_key))
+    {
+        remapped_key = stripBlackboardPointer(remapped_key);
+    }
+    config_.blackboard->set(static_cast<std::string>(remapped_key), value);
+
+    return {};
+}
+
+// Utility function to fill the list of ports using T::providedPorts();
+template <typename T>
+inline void assignDefaultRemapping(NodeConfiguration& config)
+{
+    for (const auto& it : getProvidedPorts<T>())
+    {
+        const auto& port_name = it.first;
+        const auto direction = it.second.direction();
+        if (direction != PortDirection::OUTPUT)
+        {
+            config.input_ports[port_name] = "=";
+        }
+        if (direction != PortDirection::INPUT)
+        {
+            config.output_ports[port_name] = "=";
+        }
+    }
+}
+
+}   // namespace BT
+
+#endif
diff --git a/include/behaviortree_cpp_v3/utils/any.hpp b/include/behaviortree_cpp_v3/utils/any.hpp
new file mode 100644 (file)
index 0000000..9c55bee
--- /dev/null
@@ -0,0 +1,463 @@
+//
+// Implementation of N4562 std::experimental::any (merged into C++17) for C++11 compilers.
+//
+// See also:
+//   + http://en.cppreference.com/w/cpp/any
+//   + http://en.cppreference.com/w/cpp/experimental/any
+//   + http://www.open-std.org/jtc1/sc22/wg21/docs/papers/2015/n4562.html#any
+//   + https://cplusplus.github.io/LWG/lwg-active.html#2509
+//
+//
+// Copyright (c) 2016 Denilson das Merc�s Amorim
+//
+// Distributed under the Boost Software License, Version 1.0. (See accompanying
+// file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
+//
+#ifndef LINB_ANY_HPP
+#define LINB_ANY_HPP
+#pragma once
+#include <typeinfo>
+#include <type_traits>
+#include <stdexcept>
+
+namespace linb
+{
+class bad_any_cast : public std::bad_cast
+{
+  public:
+    const char* what() const noexcept override
+    {
+        return "bad any cast";
+    }
+};
+
+class any final
+{
+  public:
+    /// Constructs an object of type any with an empty state.
+    any() : vtable(nullptr)
+    {
+    }
+
+    /// Constructs an object of type any with an equivalent state as other.
+    any(const any& rhs) : vtable(rhs.vtable)
+    {
+        if (!rhs.empty())
+        {
+            rhs.vtable->copy(rhs.storage, this->storage);
+        }
+    }
+
+    /// Constructs an object of type any with a state equivalent to the original state of other.
+    /// rhs is left in a valid but otherwise unspecified state.
+    any(any&& rhs) noexcept : vtable(rhs.vtable)
+    {
+        if (!rhs.empty())
+        {
+            rhs.vtable->move(rhs.storage, this->storage);
+            rhs.vtable = nullptr;
+        }
+    }
+
+    /// Same effect as this->clear().
+    ~any()
+    {
+        this->clear();
+    }
+
+    /// Constructs an object of type any that contains an object of type T direct-initialized with std::forward<ValueType>(value).
+    ///
+    /// T shall satisfy the CopyConstructible requirements, otherwise the program is ill-formed.
+    /// This is because an `any` may be copy constructed into another `any` at any time, so a copy should always be allowed.
+    template <typename ValueType, typename = typename std::enable_if<!std::is_same<
+                                      typename std::decay<ValueType>::type, any>::value>::type>
+    any(ValueType&& value)
+    {
+        static_assert(std::is_copy_constructible<typename std::decay<ValueType>::type>::value,
+                      "T shall satisfy the CopyConstructible requirements.");
+        this->construct(std::forward<ValueType>(value));
+    }
+
+    /// Has the same effect as any(rhs).swap(*this). No effects if an exception is thrown.
+    any& operator=(const any& rhs)
+    {
+        any(rhs).swap(*this);
+        return *this;
+    }
+
+    /// Has the same effect as any(std::move(rhs)).swap(*this).
+    ///
+    /// The state of *this is equivalent to the original state of rhs and rhs is left in a valid
+    /// but otherwise unspecified state.
+    any& operator=(any&& rhs) noexcept
+    {
+        any(std::move(rhs)).swap(*this);
+        return *this;
+    }
+
+    /// Has the same effect as any(std::forward<ValueType>(value)).swap(*this). No effect if a exception is thrown.
+    ///
+    /// T shall satisfy the CopyConstructible requirements, otherwise the program is ill-formed.
+    /// This is because an `any` may be copy constructed into another `any` at any time, so a copy should always be allowed.
+    template <typename ValueType, typename = typename std::enable_if<!std::is_same<
+                                      typename std::decay<ValueType>::type, any>::value>::type>
+    any& operator=(ValueType&& value)
+    {
+        static_assert(std::is_copy_constructible<typename std::decay<ValueType>::type>::value,
+                      "T shall satisfy the CopyConstructible requirements.");
+        any(std::forward<ValueType>(value)).swap(*this);
+        return *this;
+    }
+
+    /// If not empty, destroys the contained object.
+    void clear() noexcept
+    {
+        if (!empty())
+        {
+            this->vtable->destroy(storage);
+            this->vtable = nullptr;
+        }
+    }
+
+    /// Returns true if *this has no contained object, otherwise false.
+    bool empty() const noexcept
+    {
+        return this->vtable == nullptr;
+    }
+
+    /// If *this has a contained object of type T, typeid(T); otherwise typeid(void).
+    const std::type_info& type() const noexcept
+    {
+        return empty() ? typeid(void) : this->vtable->type();
+    }
+
+    /// Exchange the states of *this and rhs.
+    void swap(any& rhs) noexcept
+    {
+        if (this->vtable != rhs.vtable)
+        {
+            any tmp(std::move(rhs));
+
+            // move from *this to rhs.
+            rhs.vtable = this->vtable;
+            if (this->vtable != nullptr)
+            {
+                this->vtable->move(this->storage, rhs.storage);
+                //this->vtable = nullptr; -- uneeded, see below
+            }
+
+            // move from tmp (previously rhs) to *this.
+            this->vtable = tmp.vtable;
+            if (tmp.vtable != nullptr)
+            {
+                tmp.vtable->move(tmp.storage, this->storage);
+                tmp.vtable = nullptr;
+            }
+        }
+        else   // same types
+        {
+            if (this->vtable != nullptr)
+                this->vtable->swap(this->storage, rhs.storage);
+        }
+    }
+
+  private:   // Storage and Virtual Method Table
+    union storage_union
+    {
+        using stack_storage_t =
+            typename std::aligned_storage<2 * sizeof(void*), std::alignment_of<void*>::value>::type;
+
+        void* dynamic;
+        stack_storage_t stack;   // 2 words for e.g. shared_ptr
+    };
+
+    /// Base VTable specification.
+    struct vtable_type
+    {
+        // Note: The caller is responssible for doing .vtable = nullptr after destructful operations
+        // such as destroy() and/or move().
+
+        /// The type of the object this vtable is for.
+        const std::type_info& (*type)() noexcept;
+
+        /// Destroys the object in the union.
+        /// The state of the union after this call is unspecified, caller must ensure not to use src anymore.
+        void (*destroy)(storage_union&) noexcept;
+
+        /// Copies the **inner** content of the src union into the yet unitialized dest union.
+        /// As such, both inner objects will have the same state, but on separate memory locations.
+        void (*copy)(const storage_union& src, storage_union& dest);
+
+        /// Moves the storage from src to the yet unitialized dest union.
+        /// The state of src after this call is unspecified, caller must ensure not to use src anymore.
+        void (*move)(storage_union& src, storage_union& dest) noexcept;
+
+        /// Exchanges the storage between lhs and rhs.
+        void (*swap)(storage_union& lhs, storage_union& rhs) noexcept;
+    };
+
+    /// VTable for dynamically allocated storage.
+    template <typename T>
+    struct vtable_dynamic
+    {
+        static const std::type_info& type() noexcept
+        {
+            return typeid(T);
+        }
+
+        static void destroy(storage_union& storage) noexcept
+        {
+            //assert(reinterpret_cast<T*>(storage.dynamic));
+            delete reinterpret_cast<T*>(storage.dynamic);
+        }
+
+        static void copy(const storage_union& src, storage_union& dest)
+        {
+            dest.dynamic = new T(*reinterpret_cast<const T*>(src.dynamic));
+        }
+
+        static void move(storage_union& src, storage_union& dest) noexcept
+        {
+            dest.dynamic = src.dynamic;
+            src.dynamic = nullptr;
+        }
+
+        static void swap(storage_union& lhs, storage_union& rhs) noexcept
+        {
+            // just exchage the storage pointers.
+            std::swap(lhs.dynamic, rhs.dynamic);
+        }
+    };
+
+    /// VTable for stack allocated storage.
+    template <typename T>
+    struct vtable_stack
+    {
+        static const std::type_info& type() noexcept
+        {
+            return typeid(T);
+        }
+
+        static void destroy(storage_union& storage) noexcept
+        {
+            reinterpret_cast<T*>(&storage.stack)->~T();
+        }
+
+        static void copy(const storage_union& src, storage_union& dest)
+        {
+            new (&dest.stack) T(reinterpret_cast<const T&>(src.stack));
+        }
+
+        static void move(storage_union& src, storage_union& dest) noexcept
+        {
+            // one of the conditions for using vtable_stack is a nothrow move constructor,
+            // so this move constructor will never throw a exception.
+            new (&dest.stack) T(std::move(reinterpret_cast<T&>(src.stack)));
+            destroy(src);
+        }
+
+        static void swap(storage_union& lhs, storage_union& rhs) noexcept
+        {
+            storage_union tmp_storage;
+            move(rhs, tmp_storage);
+            move(lhs, rhs);
+            move(tmp_storage, lhs);
+        }
+    };
+
+    /// Whether the type T must be dynamically allocated or can be stored on the stack.
+    template <typename T>
+    struct requires_allocation
+      : std::integral_constant<
+            bool,
+            !(std::is_nothrow_move_constructible<T>::value   // N4562 ï¿½6.3/3 [any.class]
+              && sizeof(T) <= sizeof(storage_union::stack) &&
+              std::alignment_of<T>::value <=
+                  std::alignment_of<storage_union::stack_storage_t>::value)>
+    {
+    };
+
+    /// Returns the pointer to the vtable of the type T.
+    template <typename T>
+    static vtable_type* vtable_for_type()
+    {
+        using VTableType = typename std::conditional<requires_allocation<T>::value,
+                                                     vtable_dynamic<T>, vtable_stack<T>>::type;
+        static vtable_type table = {
+            VTableType::type, VTableType::destroy, VTableType::copy,
+            VTableType::move, VTableType::swap,
+        };
+        return &table;
+    }
+
+  protected:
+    template <typename T>
+    friend const T* any_cast(const any* operand) noexcept;
+    template <typename T>
+    friend T* any_cast(any* operand) noexcept;
+
+    /// Same effect as is_same(this->type(), t);
+    bool is_typed(const std::type_info& t) const
+    {
+        return is_same(this->type(), t);
+    }
+
+    /// Checks if two type infos are the same.
+    ///
+    /// If ANY_IMPL_FAST_TYPE_INFO_COMPARE is defined, checks only the address of the
+    /// type infos, otherwise does an actual comparision. Checking addresses is
+    /// only a valid approach when there's no interaction with outside sources
+    /// (other shared libraries and such).
+    static bool is_same(const std::type_info& a, const std::type_info& b)
+    {
+#ifdef ANY_IMPL_FAST_TYPE_INFO_COMPARE
+        return &a == &b;
+#else
+        return a == b;
+#endif
+    }
+
+    /// Casts (with no type_info checks) the storage pointer as const T*.
+    template <typename T>
+    const T* cast() const noexcept
+    {
+        return requires_allocation<typename std::decay<T>::type>::value ?
+                   reinterpret_cast<const T*>(storage.dynamic) :
+                   reinterpret_cast<const T*>(&storage.stack);
+    }
+
+    /// Casts (with no type_info checks) the storage pointer as T*.
+    template <typename T>
+    T* cast() noexcept
+    {
+        return requires_allocation<typename std::decay<T>::type>::value ?
+                   reinterpret_cast<T*>(storage.dynamic) :
+                   reinterpret_cast<T*>(&storage.stack);
+    }
+
+  private:
+    storage_union storage;   // on offset(0) so no padding for align
+    vtable_type* vtable;
+
+    template <typename ValueType, typename T>
+    typename std::enable_if<requires_allocation<T>::value>::type do_construct(ValueType&& value)
+    {
+        storage.dynamic = new T(std::forward<ValueType>(value));
+    }
+
+    template <typename ValueType, typename T>
+    typename std::enable_if<!requires_allocation<T>::value>::type do_construct(ValueType&& value)
+    {
+        new (&storage.stack) T(std::forward<ValueType>(value));
+    }
+
+    /// Chooses between stack and dynamic allocation for the type decay_t<ValueType>,
+    /// assigns the correct vtable, and constructs the object on our storage.
+    template <typename ValueType>
+    void construct(ValueType&& value)
+    {
+        using T = typename std::decay<ValueType>::type;
+
+        this->vtable = vtable_for_type<T>();
+
+        do_construct<ValueType, T>(std::forward<ValueType>(value));
+    }
+};
+
+namespace detail
+{
+template <typename ValueType>
+inline ValueType any_cast_move_if_true(typename std::remove_reference<ValueType>::type* p,
+                                       std::true_type)
+{
+    return std::move(*p);
+}
+
+template <typename ValueType>
+inline ValueType any_cast_move_if_true(typename std::remove_reference<ValueType>::type* p,
+                                       std::false_type)
+{
+    return *p;
+}
+}
+
+/// Performs *any_cast<add_const_t<remove_reference_t<ValueType>>>(&operand), or throws bad_any_cast on failure.
+template <typename ValueType>
+inline ValueType any_cast(const any& operand)
+{
+    auto p =
+        any_cast<typename std::add_const<typename std::remove_reference<ValueType>::type>::type>(
+            &operand);
+    if (p == nullptr)
+        throw bad_any_cast();
+    return *p;
+}
+
+/// Performs *any_cast<remove_reference_t<ValueType>>(&operand), or throws bad_any_cast on failure.
+template <typename ValueType>
+inline ValueType any_cast(any& operand)
+{
+    auto p = any_cast<typename std::remove_reference<ValueType>::type>(&operand);
+    if (p == nullptr)
+        throw bad_any_cast();
+    return *p;
+}
+
+///
+/// If ANY_IMPL_ANYCAST_MOVEABLE is not defined, does as N4562 specifies:
+///     Performs *any_cast<remove_reference_t<ValueType>>(&operand), or throws bad_any_cast on failure.
+///
+/// If ANY_IMPL_ANYCAST_MOVEABLE is defined, does as LWG Defect 2509 specifies:
+///     If ValueType is MoveConstructible and isn't a lvalue reference, performs
+///     std::move(*any_cast<remove_reference_t<ValueType>>(&operand)), otherwise
+///     *any_cast<remove_reference_t<ValueType>>(&operand). Throws bad_any_cast on failure.
+///
+template <typename ValueType>
+inline ValueType any_cast(any&& operand)
+{
+#ifdef ANY_IMPL_ANY_CAST_MOVEABLE
+    // https://cplusplus.github.io/LWG/lwg-active.html#2509
+    using can_move = std::integral_constant<bool, std::is_move_constructible<ValueType>::value &&
+                                                      !std::is_lvalue_reference<ValueType>::value>;
+#else
+    using can_move = std::false_type;
+#endif
+
+    auto p = any_cast<typename std::remove_reference<ValueType>::type>(&operand);
+    if (p == nullptr)
+        throw bad_any_cast();
+    return detail::any_cast_move_if_true<ValueType>(p, can_move());
+}
+
+/// If operand != nullptr && operand->type() == typeid(ValueType), a pointer to the object
+/// contained by operand, otherwise nullptr.
+template <typename T>
+inline const T* any_cast(const any* operand) noexcept
+{
+    if (operand == nullptr || !operand->is_typed(typeid(T)))
+        return nullptr;
+    else
+        return operand->cast<T>();
+}
+
+/// If operand != nullptr && operand->type() == typeid(ValueType), a pointer to the object
+/// contained by operand, otherwise nullptr.
+template <typename T>
+inline T* any_cast(any* operand) noexcept
+{
+    if (operand == nullptr || !operand->is_typed(typeid(T)))
+        return nullptr;
+    else
+        return operand->cast<T>();
+}
+}
+
+namespace std
+{
+inline void swap(linb::any& lhs, linb::any& rhs) noexcept
+{
+    lhs.swap(rhs);
+}
+}
+
+#endif
diff --git a/include/behaviortree_cpp_v3/utils/convert_impl.hpp b/include/behaviortree_cpp_v3/utils/convert_impl.hpp
new file mode 100644 (file)
index 0000000..35c4263
--- /dev/null
@@ -0,0 +1,302 @@
+#ifndef CONVERT_IMPL_HPP
+#define CONVERT_IMPL_HPP
+
+#include <type_traits>
+#include <exception>
+#include "simple_string.hpp"
+
+namespace SafeAny
+{
+namespace details
+{
+template <typename BoolCondition>
+using EnableIf = typename std::enable_if<BoolCondition::value, void>::type;
+
+template <typename T>
+struct is_integer
+  : std::integral_constant<bool, std::is_integral<T>::value && !std::is_same<T, bool>::value &&
+                                     !std::is_same<T, char>::value>
+{
+};
+
+template <typename From, typename To>
+struct is_same_real
+  : std::integral_constant<bool, std::is_same<From, To>::value && std::is_floating_point<To>::value>
+{
+};
+
+template <typename From, typename To>
+struct is_safe_integer_conversion
+  : std::integral_constant<bool, is_integer<From>::value && is_integer<To>::value &&
+                                     sizeof(From) < sizeof(To) &&
+                                     std::is_signed<From>::value == std::is_signed<To>::value>
+{
+};
+
+template <typename T>
+struct is_convertible_type
+  : std::integral_constant<bool, std::is_integral<T>::value || std::is_floating_point<T>::value ||
+                                     std::is_same<T, bool>::value || std::is_same<T, char>::value ||
+                                     std::is_same<T, std::string>::value ||
+                                     std::is_same<T, SimpleString>::value || std::is_enum<T>::value>
+{
+};
+
+template <typename From, typename To>
+struct float_conversion : std::integral_constant<bool, std::is_floating_point<From>::value &&
+                                                           std::is_floating_point<To>::value &&
+                                                           !std::is_same<From, To>::value>
+{
+};
+
+template <typename From, typename To>
+struct unsigned_to_smaller_conversion
+  : std::integral_constant<bool, is_integer<From>::value && is_integer<To>::value &&
+                                     (sizeof(From) > sizeof(To)) && !std::is_signed<From>::value &&
+                                     !std::is_signed<To>::value>
+{
+};
+
+template <typename From, typename To>
+struct signed_to_smaller_conversion
+  : std::integral_constant<bool, is_integer<From>::value && is_integer<To>::value &&
+                                     (sizeof(From) > sizeof(To)) && std::is_signed<From>::value &&
+                                     std::is_signed<To>::value>
+{
+};
+
+//---------------------------
+template <typename From, typename To>
+struct signed_to_smaller_unsigned_conversion
+  : std::integral_constant<bool, is_integer<From>::value && is_integer<To>::value &&
+                                     sizeof(From) >= sizeof(To) && std::is_signed<From>::value &&
+                                     !std::is_signed<To>::value>
+{
+};
+
+template <typename From, typename To>
+struct signed_to_larger_unsigned_conversion
+  : std::integral_constant<bool, is_integer<From>::value && is_integer<To>::value &&
+                                     sizeof(From) < sizeof(To) && std::is_signed<From>::value &&
+                                     !std::is_signed<To>::value>
+{
+};
+
+template <typename From, typename To>
+struct unsigned_to_smaller_signed_conversion
+  : std::integral_constant<bool, is_integer<From>::value && is_integer<To>::value &&
+                                     (sizeof(From) >= sizeof(To)) && !std::is_signed<From>::value &&
+                                     std::is_signed<To>::value>
+{
+};
+
+template <typename From, typename To>
+struct unsigned_to_larger_signed_conversion
+  : std::integral_constant<bool, is_integer<From>::value && is_integer<To>::value &&
+                                     sizeof(From) < sizeof(To) && !std::is_signed<From>::value &&
+                                     std::is_signed<To>::value>
+{
+};
+
+template <typename From, typename To>
+struct floating_to_signed_conversion
+  : std::integral_constant<bool, std::is_floating_point<From>::value && is_integer<To>::value &&
+                                     std::is_signed<To>::value>
+{
+};
+
+template <typename From, typename To>
+struct floating_to_unsigned_conversion
+  : std::integral_constant<bool, std::is_floating_point<From>::value && is_integer<To>::value &&
+                                     !std::is_signed<To>::value>
+{
+};
+
+template <typename From, typename To>
+struct integer_to_floating_conversion
+  : std::integral_constant<bool, is_integer<From>::value && std::is_floating_point<To>::value>
+{
+};
+
+template <typename From, typename To>
+inline void checkUpperLimit(const From& from)
+{
+    if ((sizeof(To) < sizeof(From)) && (from > static_cast<From>(std::numeric_limits<To>::max())))
+    {
+        throw std::runtime_error("Value too large.");
+    }
+    else if (static_cast<To>(from) > std::numeric_limits<To>::max())
+    {
+        throw std::runtime_error("Value too large.");
+    }
+}
+
+template <typename From, typename To>
+inline void checkUpperLimitFloat(const From& from)
+{
+    if (from > std::numeric_limits<To>::max())
+    {
+        throw std::runtime_error("Value too large.");
+    }
+}
+
+template <typename From, typename To>
+inline void checkLowerLimitFloat(const From& from)
+{
+    if ( from < -std::numeric_limits<To>::max())
+    {
+        throw std::runtime_error("Value too small.");
+    }
+}
+
+template <typename From, typename To>
+inline void checkLowerLimit(const From& from)
+{
+    if (from < std::numeric_limits<To>::min())
+    {
+        throw std::runtime_error("Value too small.");
+    }
+}
+
+template <typename From, typename To>
+inline void checkTruncation(const From& from)
+{
+    if (from != static_cast<From>(static_cast<To>(from)))
+    {
+        throw std::runtime_error("Floating point truncated");
+    }
+}
+
+//----------------------- Implementation ----------------------------------------------
+
+template <typename SRC, typename DST>
+inline typename std::enable_if<!is_convertible_type<DST>::value, void>::type
+convertNumber(const SRC&, DST&)
+{
+    static_assert(is_convertible_type<DST>::value, "Not convertible");
+}
+
+template <typename SRC, typename DST>
+inline EnableIf<std::is_same<bool, DST>> convertNumber(const SRC& from, DST& target)
+{
+    target = (from != 0);
+}
+
+template <typename SRC, typename DST>
+inline EnableIf<std::is_same<SRC, DST>> convertNumber(const SRC& from, DST& target)
+{
+    target = from;
+}
+
+template <typename SRC, typename DST>
+inline EnableIf<is_safe_integer_conversion<SRC, DST>> convertNumber(const SRC& from, DST& target)
+{
+    target = static_cast<DST>(from);
+}
+
+template <typename SRC, typename DST>
+inline EnableIf<float_conversion<SRC, DST>> convertNumber(const SRC& from, DST& target)
+{
+    checkTruncation<SRC, DST>(from);
+    target = static_cast<DST>(from);
+}
+
+template <typename SRC, typename DST>
+inline EnableIf<unsigned_to_smaller_conversion<SRC, DST>> convertNumber(const SRC& from,
+                                                                        DST& target)
+{
+    checkUpperLimit<SRC, DST>(from);
+    target = static_cast<DST>(from);
+}
+
+template <typename SRC, typename DST>
+inline EnableIf<signed_to_smaller_conversion<SRC, DST>> convertNumber(const SRC& from, DST& target)
+{
+    checkLowerLimit<SRC, DST>(from);
+    checkUpperLimit<SRC, DST>(from);
+    target = static_cast<DST>(from);
+}
+
+template <typename SRC, typename DST>
+inline EnableIf<signed_to_smaller_unsigned_conversion<SRC, DST>> convertNumber(const SRC& from,
+                                                                               DST& target)
+{
+    if (from < 0)
+    {
+        throw std::runtime_error("Value is negative and can't be converted to signed");
+    }
+
+    checkUpperLimit<SRC, DST>(from);
+    target = static_cast<DST>(from);
+}
+
+template <typename SRC, typename DST>
+inline EnableIf<signed_to_larger_unsigned_conversion<SRC, DST>> convertNumber(const SRC& from,
+                                                                              DST& target)
+{
+    if (from < 0)
+    {
+        throw std::runtime_error("Value is negative and can't be converted to signed");
+    }
+
+    target = static_cast<DST>(from);
+}
+
+template <typename SRC, typename DST>
+inline EnableIf<unsigned_to_larger_signed_conversion<SRC, DST>> convertNumber(const SRC& from,
+                                                                              DST& target)
+{
+    target = static_cast<DST>(from);
+}
+
+template <typename SRC, typename DST>
+inline EnableIf<unsigned_to_smaller_signed_conversion<SRC, DST>> convertNumber(const SRC& from,
+                                                                               DST& target)
+{
+    checkUpperLimit<SRC, DST>(from);
+    target = static_cast<DST>(from);
+}
+
+template <typename SRC, typename DST>
+inline EnableIf<floating_to_signed_conversion<SRC, DST>> convertNumber(const SRC& from, DST& target)
+{
+    checkLowerLimitFloat<SRC, DST>(from);
+    checkLowerLimitFloat<SRC, DST>(from);
+
+    if (from != static_cast<SRC>(static_cast<DST>(from)))
+    {
+        throw std::runtime_error("Floating point truncated");
+    }
+
+    target = static_cast<DST>(from);
+}
+
+template <typename SRC, typename DST>
+inline EnableIf<floating_to_unsigned_conversion<SRC, DST>> convertNumber(const SRC& from,
+                                                                         DST& target)
+{
+    if (from < 0)
+    {
+        throw std::runtime_error("Value is negative and can't be converted to signed");
+    }
+
+    if (from != static_cast<SRC>(static_cast<DST>(from)))
+    {
+        throw std::runtime_error("Floating point truncated");
+    }
+
+    target = static_cast<DST>(from);
+}
+
+template <typename SRC, typename DST>
+inline EnableIf<integer_to_floating_conversion<SRC, DST>> convertNumber(const SRC& from,
+                                                                        DST& target)
+{
+    checkTruncation<SRC, DST>(from);
+    target = static_cast<DST>(from);
+}
+
+}   //end namespace details
+}   //end namespace details
+
+#endif   // CONVERT_IMPL_HPP
diff --git a/include/behaviortree_cpp_v3/utils/demangle_util.h b/include/behaviortree_cpp_v3/utils/demangle_util.h
new file mode 100644 (file)
index 0000000..fff7301
--- /dev/null
@@ -0,0 +1,116 @@
+#ifndef DEMANGLE_UTIL_H
+#define DEMANGLE_UTIL_H
+
+#include <string>
+
+
+#if defined( __clang__ ) && defined( __has_include )
+# if __has_include(<cxxabi.h>)
+#  define HAS_CXXABI_H
+# endif
+#elif defined( __GLIBCXX__ ) || defined( __GLIBCPP__ )
+# define HAS_CXXABI_H
+#endif
+
+#if defined( HAS_CXXABI_H )
+# include <cxxabi.h>
+# include <cstdlib>
+# include <cstddef>
+#endif
+
+namespace BT
+{
+
+inline char const * demangle_alloc( char const * name ) noexcept;
+inline void demangle_free( char const * name ) noexcept;
+
+class scoped_demangled_name
+{
+private:
+    char const * m_p;
+
+public:
+    explicit scoped_demangled_name( char const * name ) noexcept :
+        m_p( demangle_alloc( name ) )
+    {
+    }
+
+    ~scoped_demangled_name() noexcept
+    {
+        demangle_free( m_p );
+    }
+
+    char const * get() const noexcept
+    {
+        return m_p;
+    }
+
+    scoped_demangled_name( scoped_demangled_name const& ) = delete;
+    scoped_demangled_name& operator= ( scoped_demangled_name const& ) = delete;
+};
+
+
+#if defined( HAS_CXXABI_H )
+
+inline char const * demangle_alloc( char const * name ) noexcept
+{
+    int status = 0;
+    std::size_t size = 0;
+    return abi::__cxa_demangle( name, NULL, &size, &status );
+}
+
+inline void demangle_free( char const * name ) noexcept
+{
+    std::free( const_cast< char* >( name ) );
+}
+
+#else
+
+inline char const * demangle_alloc( char const * name ) noexcept
+{
+    return name;
+}
+
+inline void demangle_free( char const * ) noexcept
+{
+}
+
+inline std::string demangle( char const * name )
+{
+    return name;
+}
+
+#endif
+
+inline std::string demangle(const std::type_info* info)
+{
+    if (!info)
+    {
+        return "void";
+    }
+    if (info == &typeid(std::string))
+    {
+        return "std::string";
+    }
+    scoped_demangled_name demangled_name(info->name());
+    char const* const p = demangled_name.get();
+    if (p)
+    {
+        return p;
+    }
+    else
+    {
+        return info->name();
+    }
+}
+
+inline std::string demangle(const std::type_info& info)
+{
+    return demangle(&info);
+}
+
+} // namespace BT
+
+#undef HAS_CXXABI_H
+
+#endif // DEMANGLE_UTIL_H
diff --git a/include/behaviortree_cpp_v3/utils/expected.hpp b/include/behaviortree_cpp_v3/utils/expected.hpp
new file mode 100644 (file)
index 0000000..65e089c
--- /dev/null
@@ -0,0 +1,1957 @@
+// This version targets C++11 and later.
+//
+// Copyright (C) 2016-2018 Martin Moene.
+//
+// Distributed under the Boost Software License, Version 1.0.
+// (See accompanying file LICENSE.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
+//
+// expected lite is based on:
+//   A proposal to add a utility class to represent expected monad
+//   by Vicente J. Botet Escriba and Pierre Talbot. http:://wg21.link/p0323
+
+#ifndef NONSTD_EXPECTED_LITE_HPP
+#define NONSTD_EXPECTED_LITE_HPP
+
+#define expected_lite_MAJOR  0
+#define expected_lite_MINOR  2
+#define expected_lite_PATCH  0
+
+#define expected_lite_VERSION  expected_STRINGIFY(expected_lite_MAJOR) "." expected_STRINGIFY(expected_lite_MINOR) "." expected_STRINGIFY(expected_lite_PATCH)
+
+#define expected_STRINGIFY(  x )  expected_STRINGIFY_( x )
+#define expected_STRINGIFY_( x )  #x
+
+// expected-lite configuration:
+
+#define nsel_EXPECTED_DEFAULT  0
+#define nsel_EXPECTED_NONSTD   1
+#define nsel_EXPECTED_STD      2
+
+#if !defined( nsel_CONFIG_SELECT_EXPECTED )
+# define nsel_CONFIG_SELECT_EXPECTED  ( nsel_HAVE_STD_EXPECTED ? nsel_EXPECTED_STD : nsel_EXPECTED_NONSTD )
+#endif
+
+// Proposal revisions:
+//
+// DXXXXR0: --
+// N4015  : -2 (2014-05-26)
+// N4109  : -1 (2014-06-29)
+// P0323R0:  0 (2016-05-28)
+// P0323R1:  1 (2016-10-12)
+// -------:
+// P0323R2:  2 (2017-06-15)
+// P0323R3:  3 (2017-10-15)
+// P0323R4:  4 (2017-11-26)
+// P0323R5:  5 (2018-02-08) *
+// P0323R6:  6 (2018-04-02)
+// P0323R7:  7 (2018-06-22)
+//
+// expected-lite uses 2 and higher
+
+#ifndef  nsel_P0323R
+# define nsel_P0323R  5
+#endif
+
+// C++ language version detection (C++20 is speculative):
+// Note: VC14.0/1900 (VS2015) lacks too much from C++14.
+
+#ifndef   nsel_CPLUSPLUS
+# if defined(_MSVC_LANG ) && !defined(__clang__)
+#  define nsel_CPLUSPLUS  (_MSC_VER == 1900 ? 201103L : _MSVC_LANG )
+# else
+#  define nsel_CPLUSPLUS  __cplusplus
+# endif
+#endif
+
+#define nsel_CPP98_OR_GREATER  ( nsel_CPLUSPLUS >= 199711L )
+#define nsel_CPP11_OR_GREATER  ( nsel_CPLUSPLUS >= 201103L )
+#define nsel_CPP14_OR_GREATER  ( nsel_CPLUSPLUS >= 201402L )
+#define nsel_CPP17_OR_GREATER  ( nsel_CPLUSPLUS >= 201703L )
+#define nsel_CPP20_OR_GREATER  ( nsel_CPLUSPLUS >= 202000L )
+
+// Use C++20 std::expected if available and requested:
+
+#if nsel_CPP20_OR_GREATER && defined(__has_include )
+# if __has_include( <expected> )
+#  define nsel_HAVE_STD_EXPECTED  1
+# else
+#  define nsel_HAVE_STD_EXPECTED  0
+# endif
+#else
+# define  nsel_HAVE_STD_EXPECTED  0
+#endif
+
+#define  nsel_USES_STD_EXPECTED  ( (nsel_CONFIG_SELECT_EXPECTED == nsel_EXPECTED_STD) || ((nsel_CONFIG_SELECT_EXPECTED == nsel_EXPECTED_DEFAULT) && nsel_HAVE_STD_EXPECTED) )
+
+//
+// in_place: code duplicated in any-lite, expected-lite, optional-lite, value-ptr-lite, variant-lite:
+//
+
+#ifndef nonstd_lite_HAVE_IN_PLACE_TYPES
+#define nonstd_lite_HAVE_IN_PLACE_TYPES  1
+
+// C++17 std::in_place in <utility>:
+
+#if nsel_CPP17_OR_GREATER
+
+#include <utility>
+
+namespace nonstd {
+
+using std::in_place;
+using std::in_place_type;
+using std::in_place_index;
+using std::in_place_t;
+using std::in_place_type_t;
+using std::in_place_index_t;
+
+#define nonstd_lite_in_place_t(      T)  std::in_place_t
+#define nonstd_lite_in_place_type_t( T)  std::in_place_type_t<T>
+#define nonstd_lite_in_place_index_t(K)  std::in_place_index_t<K>
+
+#define nonstd_lite_in_place(      T)    std::in_place_t{}
+#define nonstd_lite_in_place_type( T)    std::in_place_type_t<T>{}
+#define nonstd_lite_in_place_index(K)    std::in_place_index_t<K>{}
+
+} // namespace nonstd
+
+#else // nsel_CPP17_OR_GREATER
+
+#include <cstddef>
+
+namespace nonstd {
+namespace detail {
+
+template< class T >
+struct in_place_type_tag {};
+
+template< std::size_t K >
+struct in_place_index_tag {};
+
+} // namespace detail
+
+struct in_place_t {};
+
+template< class T >
+inline in_place_t in_place( detail::in_place_type_tag<T> = detail::in_place_type_tag<T>() )
+{
+    return in_place_t();
+}
+
+template< std::size_t K >
+inline in_place_t in_place( detail::in_place_index_tag<K> = detail::in_place_index_tag<K>() )
+{
+    return in_place_t();
+}
+
+template< class T >
+inline in_place_t in_place_type( detail::in_place_type_tag<T> = detail::in_place_type_tag<T>() )
+{
+    return in_place_t();
+}
+
+template< std::size_t K >
+inline in_place_t in_place_index( detail::in_place_index_tag<K> = detail::in_place_index_tag<K>() )
+{
+    return in_place_t();
+}
+
+// mimic templated typedef:
+
+#define nonstd_lite_in_place_t(      T)  nonstd::in_place_t(&)( nonstd::detail::in_place_type_tag<T>  )
+#define nonstd_lite_in_place_type_t( T)  nonstd::in_place_t(&)( nonstd::detail::in_place_type_tag<T>  )
+#define nonstd_lite_in_place_index_t(K)  nonstd::in_place_t(&)( nonstd::detail::in_place_index_tag<K> )
+
+#define nonstd_lite_in_place(      T)    nonstd::in_place_type<T>
+#define nonstd_lite_in_place_type( T)    nonstd::in_place_type<T>
+#define nonstd_lite_in_place_index(K)    nonstd::in_place_index<K>
+
+} // namespace nonstd
+
+#endif // nsel_CPP17_OR_GREATER
+#endif // nonstd_lite_HAVE_IN_PLACE_TYPES
+
+//
+// Using std::expected:
+//
+
+#if nsel_USES_STD_EXPECTED
+
+#include <expected>
+
+namespace nonstd {
+
+    using std::expected;
+//  ...
+}
+
+#else // nsel_USES_STD_EXPECTED
+
+#include <cassert>
+#include <exception>
+#include <functional>
+#include <initializer_list>
+#include <new>
+#include <stdexcept>
+#include <system_error>
+#include <type_traits>
+#include <utility>
+
+#if nsel_CPP11_OR_GREATER
+# define nsel_constexpr  constexpr
+#else
+# define nsel_constexpr  /*constexpr*/
+#endif
+
+#if nsel_CPP14_OR_GREATER
+# define nsel_constexpr14 constexpr
+#else
+# define nsel_constexpr14 /*constexpr*/
+#endif
+
+#if nsel_CPP17_OR_GREATER
+# define nsel_inline17 inline
+#else
+# define nsel_inline17 /*inline*/
+#endif
+
+// Method enabling
+
+#define nsel_REQUIRES_A(...) \
+    , typename std::enable_if<__VA_ARGS__, void*>::type = nullptr
+
+#define nsel_REQUIRES_0(...) \
+    template< bool B = (__VA_ARGS__), typename std::enable_if<B, int>::type = 0 >
+
+#define nsel_REQUIRES_R(R, ...) \
+    typename std::enable_if<__VA_ARGS__, R>::type
+
+#define nsel_REQUIRES_T(...) \
+    , typename = typename std::enable_if< (__VA_ARGS__), nonstd::expected_lite::detail::enabler >::type
+
+// Compiler versions:
+//
+// MSVC++ 6.0  _MSC_VER == 1200 (Visual Studio 6.0)
+// MSVC++ 7.0  _MSC_VER == 1300 (Visual Studio .NET 2002)
+// MSVC++ 7.1  _MSC_VER == 1310 (Visual Studio .NET 2003)
+// MSVC++ 8.0  _MSC_VER == 1400 (Visual Studio 2005)
+// MSVC++ 9.0  _MSC_VER == 1500 (Visual Studio 2008)
+// MSVC++ 10.0 _MSC_VER == 1600 (Visual Studio 2010)
+// MSVC++ 11.0 _MSC_VER == 1700 (Visual Studio 2012)
+// MSVC++ 12.0 _MSC_VER == 1800 (Visual Studio 2013)
+// MSVC++ 14.0 _MSC_VER == 1900 (Visual Studio 2015)
+// MSVC++ 14.1 _MSC_VER >= 1910 (Visual Studio 2017)
+
+#if defined(_MSC_VER) && !defined(__clang__)
+# define nsel_COMPILER_MSVC_VER      (_MSC_VER )
+# define nsel_COMPILER_MSVC_VERSION  (_MSC_VER / 10 - 10 * ( 5 + (_MSC_VER < 1900)) )
+#else
+# define nsel_COMPILER_MSVC_VER      0
+# define nsel_COMPILER_MSVC_VERSION  0
+#endif
+
+#define nsel_COMPILER_VERSION( major, minor, patch )  ( 10 * ( 10 * (major) + (minor) ) + (patch) )
+
+#if defined(__clang__)
+# define nsel_COMPILER_CLANG_VERSION  nsel_COMPILER_VERSION(__clang_major__, __clang_minor__, __clang_patchlevel__)
+#else
+# define nsel_COMPILER_CLANG_VERSION  0
+#endif
+
+#if defined(__GNUC__) && !defined(__clang__)
+# define nsel_COMPILER_GNUC_VERSION  nsel_COMPILER_VERSION(__GNUC__, __GNUC_MINOR__, __GNUC_PATCHLEVEL__)
+#else
+# define nsel_COMPILER_GNUC_VERSION  0
+#endif
+
+// half-open range [lo..hi):
+//#define nsel_BETWEEN( v, lo, hi ) ( (lo) <= (v) && (v) < (hi) )
+
+// Presence of language and library features:
+
+#ifdef _HAS_CPP0X
+# define nsel_HAS_CPP0X  _HAS_CPP0X
+#else
+# define nsel_HAS_CPP0X  0
+#endif
+
+//#define nsel_CPP11_140  (nsel_CPP11_OR_GREATER || nsel_COMPILER_MSVC_VER >= 1900)
+
+// Clang, GNUC, MSVC warning suppression macros:
+
+#ifdef __clang__
+# pragma clang diagnostic push
+#elif defined  __GNUC__
+# pragma  GCC  diagnostic push
+#endif // __clang__
+
+#if nsel_COMPILER_MSVC_VERSION >= 140
+# pragma warning( push )
+# define nsel_DISABLE_MSVC_WARNINGS(codes)  __pragma( warning(disable: codes) )
+#else
+# define nsel_DISABLE_MSVC_WARNINGS(codes)
+#endif
+
+#ifdef __clang__
+# define nsel_RESTORE_WARNINGS()  _Pragma("clang diagnostic pop")
+#elif defined __GNUC__
+# define nsel_RESTORE_WARNINGS()  _Pragma("GCC diagnostic pop")
+#elif nsel_COMPILER_MSVC_VERSION >= 140
+# define nsel_RESTORE_WARNINGS()  __pragma( warning( pop ) )
+#else
+# define nsel_RESTORE_WARNINGS()
+#endif
+
+// Suppress the following MSVC (GSL) warnings:
+// - C26409: Avoid calling new and delete explicitly, use std::make_unique<T> instead (r.11)
+
+nsel_DISABLE_MSVC_WARNINGS( 26409 )
+
+//
+// expected:
+//
+
+namespace nonstd { namespace expected_lite {
+
+namespace std20 {
+
+// type traits C++20:
+
+template< typename T >
+struct remove_cvref
+{
+    typedef typename std::remove_cv< typename std::remove_reference<T>::type >::type type;
+};
+
+} // namespace std20
+
+// forward declaration:
+
+template< typename T, typename E >
+class expected;
+
+namespace detail {
+
+/// for nsel_REQUIRES_T
+
+enum class enabler{};
+
+/// discriminated union to hold value or 'error'.
+
+template< typename T, typename E >
+union storage_t
+{
+    friend class expected<T,E>;
+
+private:
+    using value_type = T;
+    using error_type = E;
+
+    // no-op construction
+    storage_t() {}
+    ~storage_t() {}
+
+    template<class ...Args>
+    void construct_value(Args&& ...args)
+    {
+        new(&m_value) value_type(std::forward<Args>(args)...);
+    }
+
+    void destruct_value()
+    {
+        m_value.~value_type();
+    }
+
+    void construct_error( error_type const & e )
+    {
+        new( &m_error ) error_type( e );
+    }
+
+    void construct_error( error_type && e )
+    {
+        new( &m_error ) error_type( std::move( e ) );
+    }
+
+    void destruct_error()
+    {
+        m_error.~error_type();
+    }
+
+    constexpr value_type const & value() const &
+    {
+        return m_value;
+    }
+
+    value_type & value() &
+    {
+        return m_value;
+    }
+
+    constexpr value_type const && value() const &&
+    {
+        return std::move( m_value );
+    }
+
+    nsel_constexpr14 value_type && value() &&
+    {
+        return std::move( m_value );
+    }
+
+    value_type const * value_ptr() const
+    {
+        return &m_value;
+    }
+
+    value_type * value_ptr()
+    {
+        return &m_value;
+    }
+
+    error_type const & error() const
+    {
+        return m_error;
+    }
+
+    error_type & error()
+    {
+        return m_error;
+    }
+
+private:
+    value_type m_value;
+    error_type m_error;
+};
+
+/// discriminated union to hold only 'error'.
+
+template< typename E >
+union storage_t<void, E>
+{
+    friend class expected<void,E>;
+
+private:
+    using value_type = void;
+    using error_type = E;
+
+    // no-op construction
+    storage_t() {}
+    ~storage_t() {}
+
+    void construct_error( error_type const & e )
+    {
+        new( &m_error ) error_type( e );
+    }
+
+    void construct_error( error_type && e )
+    {
+        new( &m_error ) error_type( std::move( e ) );
+    }
+
+    void destruct_error()
+    {
+        m_error.~error_type();
+    }
+
+    error_type const & error() const
+    {
+        return m_error;
+    }
+
+    error_type & error()
+    {
+        return m_error;
+    }
+
+private:
+    error_type m_error;
+};
+
+} // namespace detail
+
+/// // x.x.3 Unexpected object type; unexpected_type; C++17 and later can also use aliased type unexpected.
+
+#if nsel_P0323R <= 2
+template< typename E = std::exception_ptr >
+class unexpected_type
+#else
+template< typename E >
+class unexpected_type
+#endif // nsel_P0323R
+{
+public:
+    using error_type = E;
+
+    unexpected_type() = delete;
+    constexpr unexpected_type( unexpected_type const &) = default;
+    constexpr unexpected_type( unexpected_type &&) = default;
+    nsel_constexpr14 unexpected_type& operator=( unexpected_type const &) = default;
+    nsel_constexpr14 unexpected_type& operator=( unexpected_type &&) = default;
+
+    template< typename E2
+        nsel_REQUIRES_T(
+            std::is_constructible<E,E2&&>::value
+        )
+    >
+    constexpr explicit unexpected_type( E2 && error )
+    : m_error( std::forward<E2>( error ) )
+    {}
+
+    template< typename E2 >
+    constexpr explicit unexpected_type( unexpected_type<E2> const & error
+        nsel_REQUIRES_A(
+            std::is_constructible<E,E2 const &>::value
+            && !std::is_convertible<E2 const &, E>::value /*=> explicit */ )
+        )
+    : m_error( error )
+    {}
+
+    template< typename E2 >
+    constexpr /*non-explicit*/ unexpected_type( unexpected_type<E2> const & error
+        nsel_REQUIRES_A(
+            std::is_constructible<E,E2 const &>::value
+            && std::is_convertible<E2 const &, E>::value /*=> non-explicit */ )
+        )
+    : m_error( error )
+    {}
+
+    template< typename E2 >
+    constexpr explicit unexpected_type( unexpected_type<E2> && error
+        nsel_REQUIRES_A(
+            std::is_constructible<E,E2&&>::value
+            && !std::is_convertible<E2&&, E>::value /*=> explicit */ )
+        )
+    : m_error( error )
+    {}
+
+    template< typename E2 >
+    constexpr /*non-explicit*/ unexpected_type( unexpected_type<E2> && error
+        nsel_REQUIRES_A(
+            std::is_constructible<E,E2&&>::value
+            && std::is_convertible<E2&&, E>::value /*=> non-explicit */ )
+        )
+    : m_error( error )
+    {}
+
+    nsel_constexpr14 E & value() & noexcept
+    {
+        return m_error;
+    }
+
+    constexpr E const & value() const & noexcept
+    {
+        return m_error;
+    }
+
+    nsel_constexpr14 E && value() && noexcept
+    {
+        return std::move( m_error );
+    }
+
+    constexpr E const && value() const && noexcept
+    {
+        return std::move( m_error );
+    }
+
+//      nsel_REQUIRES_A(
+//        std::is_move_constructible<E>::value
+//        && std::is_swappable<E>::value
+//      )
+
+    void swap( unexpected_type & other ) noexcept (
+#if nsel_CPP17_OR_GREATER
+        std::is_nothrow_move_constructible<E>::value
+        && std::is_nothrow_swappable<E&>::value
+#else
+        std::is_nothrow_move_constructible<E>::value
+        && noexcept ( std::swap( std::declval<E&>(), std::declval<E&>() ) )
+#endif
+    )
+    {
+        using std::swap;
+        swap( m_error, other.m_error );
+    }
+
+private:
+    error_type m_error;
+};
+
+/// class unexpected_type, std::exception_ptr specialization (P0323R2)
+
+#if nsel_P0323R <= 2
+
+template<>
+class unexpected_type< std::exception_ptr >
+{
+public:
+    using error_type = std::exception_ptr;
+
+    unexpected_type() = delete;
+
+    ~unexpected_type(){}
+
+    explicit unexpected_type( std::exception_ptr const & error )
+    : m_error( error )
+    {}
+
+    explicit unexpected_type(std::exception_ptr && error )
+    : m_error( std::move( error ) )
+    {}
+
+    template< typename E >
+    explicit unexpected_type( E error )
+    : m_error( std::make_exception_ptr( error ) )
+    {}
+
+    std::exception_ptr const & value() const
+    {
+        return m_error;
+    }
+
+    std::exception_ptr & value()
+    {
+        return m_error;
+    }
+
+private:
+    std::exception_ptr m_error;
+};
+
+#endif // nsel_P0323R
+
+/// x.x.4, Unexpected equality operators
+
+template< typename E >
+constexpr bool operator==( unexpected_type<E> const & x, unexpected_type<E> const & y )
+{
+    return x.value() == y.value();
+}
+
+template< typename E >
+constexpr bool operator!=( unexpected_type<E> const & x, unexpected_type<E> const & y )
+{
+    return ! ( x == y );
+}
+
+#if nsel_P0323R <= 2
+
+template< typename E >
+constexpr bool operator<( unexpected_type<E> const & x, unexpected_type<E> const & y )
+{
+    return x.value() < y.value();
+}
+
+template< typename E >
+constexpr bool operator>( unexpected_type<E> const & x, unexpected_type<E> const & y )
+{
+    return ( y < x );
+}
+
+template< typename E >
+constexpr bool operator<=( unexpected_type<E> const & x, unexpected_type<E> const & y )
+{
+    return ! ( y < x  );
+}
+
+template< typename E >
+constexpr bool operator>=( unexpected_type<E> const & x, unexpected_type<E> const & y )
+{
+    return ! ( x < y );
+}
+
+/// x.x.5 Specialized algorithms
+
+template< typename E >
+void swap( unexpected_type<E> & x, unexpected_type<E> & y) noexcept ( noexcept ( x.swap(y) ) )
+{
+    x.swap( y );
+}
+
+// unexpected: relational operators for std::exception_ptr:
+
+inline constexpr bool operator<( unexpected_type<std::exception_ptr> const & /*x*/, unexpected_type<std::exception_ptr> const & /*y*/ )
+{
+    return false;
+}
+
+inline constexpr bool operator>( unexpected_type<std::exception_ptr> const & /*x*/, unexpected_type<std::exception_ptr> const & /*y*/ )
+{
+    return false;
+}
+
+inline constexpr bool operator<=( unexpected_type<std::exception_ptr> const & x, unexpected_type<std::exception_ptr> const & y )
+{
+    return ( x == y );
+}
+
+inline constexpr bool operator>=( unexpected_type<std::exception_ptr> const & x, unexpected_type<std::exception_ptr> const & y )
+{
+    return ( x == y );
+}
+
+#endif // nsel_P0323R
+
+// unexpected: traits
+
+#if nsel_P0323R <= 3
+
+template< typename E>
+struct is_unexpected : std::false_type {};
+
+template< typename E>
+struct is_unexpected< unexpected_type<E> > : std::true_type {};
+
+#endif // nsel_P0323R
+
+// unexpected: factory
+
+// keep make_unexpected() removed in p0323r2 for pre-C++17:
+
+template< typename E>
+nsel_constexpr14 auto
+make_unexpected( E && value ) -> unexpected_type< typename std::decay<E>::type >
+{
+    return unexpected_type< typename std::decay<E>::type >( std::forward<E>(value) );
+}
+
+#if nsel_P0323R <= 3
+
+/*nsel_constexpr14*/ auto inline
+make_unexpected_from_current_exception() -> unexpected_type< std::exception_ptr >
+{
+    return unexpected_type< std::exception_ptr >( std::current_exception() );
+}
+
+#endif // nsel_P0323R
+
+/// unexpect tag, in_place_unexpected tag: construct an error
+
+struct unexpect_t{};
+using in_place_unexpected_t = unexpect_t;
+
+nsel_inline17 constexpr unexpect_t unexpect{};
+nsel_inline17 constexpr unexpect_t in_place_unexpected{};
+
+/// expected access error
+
+template< typename E >
+class bad_expected_access;
+
+template <>
+class bad_expected_access< void > : public std::exception
+{
+public:
+    explicit bad_expected_access()
+    : std::exception()
+    {}
+};
+
+template< typename E >
+class bad_expected_access : public bad_expected_access< void >
+{
+public:
+    using error_type = E;
+
+    explicit bad_expected_access( error_type error )
+    : m_error( error )
+    {}
+
+    virtual char const * what() const noexcept override
+    {
+        return "bad_expected_access";
+    }
+
+    nsel_constexpr14 error_type & error() &
+    {
+        return m_error;
+    }
+
+    constexpr error_type const & error() const &
+    {
+        return m_error;
+    }
+
+    nsel_constexpr14 error_type && error() &&
+    {
+        return std::move( m_error );
+    }
+
+    constexpr error_type const && error() const &&
+    {
+        return std::move( m_error );
+    }
+
+private:
+    error_type m_error;
+};
+
+/// class error_traits
+
+template< typename Error >
+struct error_traits
+{
+    static void rethrow( Error const & e )
+    {
+        throw bad_expected_access<Error>{ e };
+    }
+};
+
+template<>
+struct error_traits< std::exception_ptr >
+{
+    static void rethrow( std::exception_ptr const & e )
+    {
+        std::rethrow_exception( e );
+    }
+};
+
+template<>
+struct error_traits< std::error_code >
+{
+    static void rethrow( std::error_code const & e )
+    {
+        throw std::system_error( e );
+    }
+};
+
+} // namespace expected_lite
+
+// provide nonstd::unexpected_type:
+
+using expected_lite::unexpected_type;
+
+namespace expected_lite {
+
+/// class expected
+
+#if nsel_P0323R <= 2
+template< typename T, typename E = std::exception_ptr >
+class expected
+#else
+template< typename T, typename E >
+class expected
+#endif // nsel_P0323R
+{
+public:
+    using value_type = T;
+    using error_type = E;
+    using unexpected_type = nonstd::unexpected_type<E>;
+
+    template< typename U >
+    struct rebind
+    {
+        using type = expected<U, error_type>;
+    };
+
+    // x.x.4.1 constructors
+
+    nsel_REQUIRES_0(
+        std::is_default_constructible<T>::value
+    )
+    nsel_constexpr14 expected() noexcept
+    (
+        std::is_nothrow_default_constructible<T>::value
+    )
+    : has_value_( true )
+    {
+        contained.construct_value( value_type() );
+    }
+
+    nsel_constexpr14 expected( expected const & other
+//      nsel_REQUIRES_A(
+//          std::is_copy_constructible<T>::value
+//          && std::is_copy_constructible<E>::value
+//      )
+    )
+    : has_value_( other.has_value_ )
+    {
+        if ( has_value() ) contained.construct_value( other.contained.value() );
+        else               contained.construct_error( other.contained.error() );
+    }
+
+    nsel_constexpr14 expected( expected && other
+//      nsel_REQUIRES_A(
+//          std::is_move_constructible<T>::value
+//          && std::is_move_constructible<E>::value
+//      )
+    ) noexcept (
+        std::is_nothrow_move_constructible<T>::value
+        && std::is_nothrow_move_constructible<E>::value
+    )
+    : has_value_( other.has_value_ )
+    {
+        if ( has_value() ) contained.construct_value( std::move( other.contained.value() ) );
+        else               contained.construct_error( std::move( other.contained.error() ) );
+    }
+
+    template< typename U, typename G >
+    nsel_constexpr14 explicit expected( expected<U, G> const & other
+        nsel_REQUIRES_A(
+            std::is_constructible<T, const U&>::value
+            && std::is_constructible<E, const G&>::value
+            && !std::is_constructible<T, expected<U, G>&>::value
+            && !std::is_constructible<T, expected<U, G>&&>::value
+            && !std::is_constructible<T, const expected<U, G>&>::value
+            && !std::is_constructible<T, const expected<U, G>&&>::value
+            && !std::is_convertible<expected<U, G>&, T>::value
+            && !std::is_convertible<expected<U, G>&&, T>::value
+            && !std::is_convertible<const expected<U, G>&, T>::value
+            && !std::is_convertible<const expected<U, G>&&, T>::value
+            && (!std::is_convertible<U const&, T>::value || !std::is_convertible<const G&, E>::value ) /*=> explicit */ )
+        )
+    : has_value_( other.has_value_ )
+    {
+        if ( has_value() ) contained.construct_value( other.contained.value() );
+        else               contained.construct_error( other.contained.error() );
+    }
+
+    template< typename U, typename G >
+    nsel_constexpr14 /*non-explicit*/ expected( expected<U, G> const & other
+        nsel_REQUIRES_A(
+            std::is_constructible<T, const U&>::value
+            && std::is_constructible<E, const G&>::value
+            && !std::is_constructible<T, expected<U, G>&>::value
+            && !std::is_constructible<T, expected<U, G>&&>::value
+            && !std::is_constructible<T, const expected<U, G>&>::value
+            && !std::is_constructible<T, const expected<U, G>&&>::value
+            && !std::is_convertible<expected<U, G>&, T>::value
+            && !std::is_convertible<expected<U, G>&&, T>::value
+            && !std::is_convertible<const expected<U, G>&, T>::value
+            && !std::is_convertible<const expected<U, G>&&, T>::value
+            && !(!std::is_convertible<U const&, T>::value || !std::is_convertible<const G&, E>::value ) /*=> explicit */ )
+        )
+    : has_value_( other.has_value_ )
+    {
+        if ( has_value() ) contained.construct_value( other.contained.value() );
+        else               contained.construct_error( other.contained.error() );
+    }
+
+    template< typename U, typename G >
+    nsel_constexpr14 explicit expected( expected<U, G> && other
+        nsel_REQUIRES_A(
+            std::is_constructible<T, U>::value
+            && std::is_constructible<E, G>::value
+            && !std::is_constructible<T, expected<U, G>&>::value
+            && !std::is_constructible<T, expected<U, G>&&>::value
+            && !std::is_constructible<T, const expected<U, G>&>::value
+            && !std::is_constructible<T, const expected<U, G>&&>::value
+            && !std::is_convertible<expected<U, G>&, T>::value
+            && !std::is_convertible<expected<U, G>&&, T>::value
+            && !std::is_convertible<const expected<U, G>&, T>::value
+            && !std::is_convertible<const expected<U, G>&&, T>::value
+            && (!std::is_convertible<U, T>::value || !std::is_convertible<G, E>::value ) /*=> explicit */ )
+        )
+    : has_value_( other.has_value_ )
+    {
+        if ( has_value() ) contained.construct_value( std::move( other.contained.value() ) );
+        else               contained.construct_error( std::move( other.contained.error() ) );
+    }
+
+    template< typename U, typename G >
+    nsel_constexpr14 /*non-explicit*/ expected( expected<U, G> && other
+        nsel_REQUIRES_A(
+            std::is_constructible<T, U>::value
+            && std::is_constructible<E, G>::value
+            && !std::is_constructible<T, expected<U, G>&>::value
+            && !std::is_constructible<T, expected<U, G>&&>::value
+            && !std::is_constructible<T, const expected<U, G>&>::value
+            && !std::is_constructible<T, const expected<U, G>&&>::value
+            && !std::is_convertible<expected<U, G>&, T>::value
+            && !std::is_convertible<expected<U, G>&&, T>::value
+            && !std::is_convertible<const expected<U, G>&, T>::value
+            && !std::is_convertible<const expected<U, G>&&, T>::value
+            && !(!std::is_convertible<U, T>::value || !std::is_convertible<G, E>::value ) /*=> non-explicit */ )
+        )
+    : has_value_( other.has_value_ )
+    {
+        if ( has_value() ) contained.construct_value( std::move( other.contained.value() ) );
+        else               contained.construct_error( std::move( other.contained.error() ) );
+    }
+
+    nsel_constexpr14 expected( value_type const & value
+//        nsel_REQUIRES_A(
+//            std::is_copy_constructible<T>::value )
+    )
+    : has_value_( true )
+    {
+        contained.construct_value( value );
+    }
+
+    template< typename U = T >
+    nsel_constexpr14 explicit expected( U && value
+        nsel_REQUIRES_A(
+            std::is_constructible<T,U&&>::value
+            && !std::is_same<typename std20::remove_cvref<U>::type, nonstd_lite_in_place_t(U)>::value
+            && !std::is_same<expected<T,E>, typename std20::remove_cvref<U>::type>::value
+            && !std::is_same<nonstd::unexpected_type<E>, typename std20::remove_cvref<U>::type>::value
+            && !std::is_convertible<U&&,T>::value /*=> explicit */
+        )
+    ) noexcept
+    (
+        std::is_nothrow_move_constructible<U>::value &&
+        std::is_nothrow_move_constructible<E>::value
+    )
+    : has_value_( true )
+    {
+        contained.construct_value( std::forward<U>( value ) );
+    }
+
+    template< typename U = T >
+    nsel_constexpr14 expected( U && value
+        nsel_REQUIRES_A(
+            std::is_constructible<T,U&&>::value
+            && !std::is_same<typename std20::remove_cvref<U>::type, nonstd_lite_in_place_t(U)>::value
+            && !std::is_same<expected<T,E>, typename std20::remove_cvref<U>::type>::value
+            && !std::is_same<nonstd::unexpected_type<E>, typename std20::remove_cvref<U>::type>::value
+            &&  std::is_convertible<U&&,T>::value /*=> non-explicit */
+        )
+    ) noexcept
+    (
+        std::is_nothrow_move_constructible<U>::value &&
+        std::is_nothrow_move_constructible<E>::value
+    )
+    : has_value_( true )
+    {
+        contained.construct_value( std::forward<U>( value ) );
+    }
+
+    template< typename... Args
+        nsel_REQUIRES_T(
+            std::is_constructible<T, Args&&...>::value
+        )
+    >
+    nsel_constexpr14 explicit expected( nonstd_lite_in_place_t(T), Args&&... args )
+    : has_value_( true )
+    {
+        contained.construct_value( std::forward<Args>( args )... );
+    }
+
+    template< typename U, typename... Args
+        nsel_REQUIRES_T(
+            std::is_constructible<T, std::initializer_list<U>, Args&&...>::value
+        )
+    >
+    nsel_constexpr14 explicit expected( nonstd_lite_in_place_t(T), std::initializer_list<U> il, Args&&... args )
+    : has_value_( true )
+    {
+        contained.construct_value( il, std::forward<Args>( args )... );
+    }
+
+    template< typename G = E >
+    nsel_constexpr14 explicit expected( nonstd::unexpected_type<G> const & error
+        nsel_REQUIRES_A(
+            !std::is_convertible<const G&, E>::value /*=> explicit */ )
+        )
+    : has_value_( false )
+    {
+        contained.construct_error( error.value() );
+    }
+
+    template< typename G = E >
+    nsel_constexpr14 /*non-explicit*/ expected( nonstd::unexpected_type<G> const & error
+        nsel_REQUIRES_A(
+            std::is_convertible<const G&, E>::value /*=> non-explicit */ )
+        )
+    : has_value_( false )
+    {
+        contained.construct_error( error.value() );
+    }
+
+    template< typename G = E >
+    nsel_constexpr14 explicit expected( nonstd::unexpected_type<G> && error
+        nsel_REQUIRES_A(
+            !std::is_convertible<G&&, E>::value /*=> explicit */ )
+        )
+    : has_value_( false )
+    {
+        contained.construct_error( std::move( error.value() ) );
+    }
+
+    template< typename G = E >
+    nsel_constexpr14 /*non-explicit*/ expected( nonstd::unexpected_type<G> && error
+        nsel_REQUIRES_A(
+            std::is_convertible<G&&, E>::value /*=> non-explicit */ )
+        )
+    : has_value_( false )
+    {
+        contained.construct_error( std::move( error.value() ) );
+    }
+
+    template< typename... Args
+        nsel_REQUIRES_T(
+            std::is_constructible<E, Args&&...>::value
+        )
+    >
+    nsel_constexpr14 explicit expected( unexpect_t, Args&&... args )
+    : has_value_( false )
+    {
+        contained.construct_error( std::forward<Args>( args )... );
+    }
+
+    template< typename U, typename... Args
+        nsel_REQUIRES_T(
+            std::is_constructible<T, std::initializer_list<U>, Args&&...>::value
+        )
+    >
+    nsel_constexpr14 explicit expected( unexpect_t, std::initializer_list<U> il, Args&&... args )
+    : has_value_( false )
+    {
+        contained.construct_error( il, std::forward<Args>( args )... );
+    }
+
+    // x.x.4.2 destructor
+
+    ~expected()
+    {
+        if ( has_value() ) contained.destruct_value();
+        else               contained.destruct_error();
+    }
+
+    // x.x.4.3 assignment
+
+//    nsel_REQUIRES_A(
+//        std::is_copy_constructible<T>::value &&
+//        std::is_copy_assignable<T>::value &&
+//        std::is_copy_constructible<E>::value &&
+//        std::is_copy_assignable<E>::value )
+
+    expected operator=( expected const & other )
+    {
+        expected( other ).swap( *this );
+        return *this;
+    }
+
+//    nsel_REQUIRES_A(
+//        std::is_move_constructible<T>::value &&
+//        std::is_move_assignable<T>::value &&
+//        std::is_move_constructible<E>::value &&
+//        std::is_move_assignable<E>::value )
+
+    expected & operator=( expected && other ) noexcept
+    (
+        std::is_nothrow_move_assignable<T>::value &&
+        std::is_nothrow_move_constructible<T>::value&&
+        std::is_nothrow_move_assignable<E>::value &&
+        std::is_nothrow_move_constructible<E>::value )
+    {
+        expected( std::move( other ) ).swap( *this );
+        return *this;
+    }
+
+    template< typename U
+        nsel_REQUIRES_T(
+            std::is_constructible<T,U>::value &&
+            std::is_assignable<T&, U>::value
+        )
+    >
+    expected & operator=( U && value )
+    {
+        expected( std::forward<U>( value ) ).swap( *this );
+        return *this;
+    }
+
+//    nsel_REQUIRES_A(
+//        std::is_copy_constructible<E>::value &&
+//        std::is_assignable<E&, E>::value )
+
+    expected & operator=( unexpected_type const & uvalue )
+    {
+        expected( std::move( uvalue ) ).swap( *this );
+        return *this;
+    }
+
+//    nsel_REQUIRES_A(
+//        std::is_copy_constructible<E>::value &&
+//        std::is_assignable<E&, E>::value )
+
+    expected & operator=( unexpected_type && uvalue )
+    {
+        expected( std::move( uvalue ) ).swap( *this );
+        return *this;
+    }
+
+    template< typename... Args
+        nsel_REQUIRES_T(
+            std::is_constructible<T, Args&&...>::value
+        )
+    >
+    void emplace( Args &&... args )
+    {
+        expected( nonstd_lite_in_place(T), std::forward<Args>(args)... ).swap( *this );
+    }
+
+    template< typename U, typename... Args
+        nsel_REQUIRES_T(
+            std::is_constructible<T, std::initializer_list<U>&, Args&&...>::value
+        )
+    >
+    void emplace( std::initializer_list<U> il, Args &&... args )
+    {
+        expected( nonstd_lite_in_place(T), il, std::forward<Args>(args)... ).swap( *this );
+    }
+
+    // x.x.4.4 swap
+
+//    nsel_REQUIRES_A(
+//        std::is_move_constructible<T>::value &&
+//        std::is_move_constructible<E>::value )
+
+    void swap( expected & other ) noexcept
+    (
+#if nsel_CPP17_OR_GREATER
+        std::is_nothrow_move_constructible<T>::value && std::is_nothrow_swappable<T&>::value &&
+        std::is_nothrow_move_constructible<E>::value && std::is_nothrow_swappable<E&>::value
+#else
+        std::is_nothrow_move_constructible<T>::value && noexcept ( std::swap( std::declval<T&>(), std::declval<T&>() ) ) &&
+        std::is_nothrow_move_constructible<E>::value && noexcept ( std::swap( std::declval<E&>(), std::declval<E&>() ) )
+#endif
+    )
+    {
+        using std::swap;
+
+        if      (   bool(*this) &&   bool(other) ) { swap( contained.value(), other.contained.value() ); }
+        else if ( ! bool(*this) && ! bool(other) ) { swap( contained.error(), other.contained.error() ); }
+        else if (   bool(*this) && ! bool(other) ) { error_type t( std::move( other.error() ) );
+                                                   other.contained.destruct_error();
+                                                   other.contained.construct_value( std::move( contained.value() ) );
+                                                   contained.destruct_value();
+                                                   contained.construct_error( std::move( t ) );
+                                                   swap( has_value_, other.has_value_ ); }
+        else if ( ! bool(*this) &&   bool(other) ) { other.swap( *this ); }
+    }
+
+    // x.x.4.5 observers
+
+    constexpr value_type const * operator ->() const
+    {
+        return assert( has_value() ), contained.value_ptr();
+    }
+
+    value_type * operator ->()
+    {
+        return assert( has_value() ), contained.value_ptr();
+    }
+
+    constexpr value_type const & operator *() const &
+    {
+        return assert( has_value() ), contained.value();
+    }
+
+    value_type & operator *() &
+    {
+        return assert( has_value() ), contained.value();
+    }
+
+    constexpr value_type const && operator *() const &&
+    {
+        return assert( has_value() ), std::move( contained.value() );
+    }
+
+    nsel_constexpr14 value_type && operator *() &&
+    {
+        return assert( has_value() ), std::move( contained.value() );
+    }
+
+    constexpr explicit operator bool() const noexcept
+    {
+        return has_value();
+    }
+
+    constexpr bool has_value() const noexcept
+    {
+        return has_value_;
+    }
+
+    constexpr value_type const & value() const &
+    {
+        return has_value()
+            ? ( contained.value() )
+            : ( error_traits<error_type>::rethrow( contained.error() ), contained.value() );
+    }
+
+    value_type & value() &
+    {
+        return has_value()
+            ? ( contained.value() )
+            : ( error_traits<error_type>::rethrow( contained.error() ), contained.value() );
+    }
+
+    constexpr value_type const && value() const &&
+    {
+        return std::move( has_value()
+            ? ( contained.value() )
+            : ( error_traits<error_type>::rethrow( contained.error() ), contained.value() ) );
+    }
+
+    nsel_constexpr14 value_type && value() &&
+    {
+        return std::move( has_value()
+            ? ( contained.value() )
+            : ( error_traits<error_type>::rethrow( contained.error() ), contained.value() ) );
+    }
+
+    constexpr error_type const & error() const &
+    {
+        return assert( ! has_value() ), contained.error();
+    }
+
+    error_type & error() &
+    {
+        return assert( ! has_value() ), contained.error();
+    }
+
+    constexpr error_type const && error() const &&
+    {
+        return assert( ! has_value() ), std::move( contained.error() );
+    }
+
+    error_type && error() &&
+    {
+        return assert( ! has_value() ), std::move( contained.error() );
+    }
+
+    constexpr unexpected_type get_unexpected() const
+    {
+        return make_unexpected( contained.error() );
+    }
+
+    template< typename Ex >
+    bool has_exception() const
+    {
+        using ContainedEx = typename std::remove_reference< decltype( get_unexpected().value() ) >::type;
+        return ! has_value() && std::is_base_of< Ex, ContainedEx>::value;
+    }
+
+    template< typename U
+        nsel_REQUIRES_T(
+            std::is_copy_constructible<T>::value &&
+            std::is_convertible<U&&, T>::value
+        )
+    >
+    value_type value_or( U && v ) const &
+    {
+        return has_value()
+            ? contained.value()
+            : static_cast<T>( std::forward<U>( v ) );
+    }
+
+    template< typename U
+        nsel_REQUIRES_T(
+            std::is_move_constructible<T>::value &&
+            std::is_convertible<U&&, T>::value
+        )
+    >
+    value_type value_or( U && v ) &&
+    {
+        return has_value()
+            ? std::move( contained.value() )
+            : static_cast<T>( std::forward<U>( v ) );
+    }
+
+    // unwrap()
+
+//  template <class U, class E>
+//  constexpr expected<U,E> expected<expected<U,E>,E>::unwrap() const&;
+
+//  template <class T, class E>
+//  constexpr expected<T,E> expected<T,E>::unwrap() const&;
+
+//  template <class U, class E>
+//  expected<U,E> expected<expected<U,E>, E>::unwrap() &&;
+
+//  template <class T, class E>
+//  template expected<T,E> expected<T,E>::unwrap() &&;
+
+    // factories
+
+//  template< typename Ex, typename F>
+//  expected<T,E> catch_exception(F&& f);
+
+//  template< typename F>
+//  expected<decltype(func(declval<T>())),E> map(F&& func) ;
+
+//  template< typename F>
+//  'see below' bind(F&& func);
+
+//  template< typename F>
+//  expected<T,E> catch_error(F&& f);
+
+//  template< typename F>
+//  'see below' then(F&& func);
+
+private:
+    bool has_value_;
+    detail::storage_t<T,E> contained;
+};
+
+/// class expected, void specialization
+
+template< typename E >
+class expected<void, E>
+{
+public:
+    using value_type = void;
+    using error_type = E;
+    using unexpected_type = nonstd::unexpected_type<E>;
+
+    // x.x.4.1 constructors
+
+    constexpr expected() noexcept
+    : has_value_( true )
+    {
+    }
+
+    nsel_constexpr14 expected( expected const & other )
+    : has_value_( other.has_value_ )
+    {
+        if ( ! has_value() ) contained.construct_error( other.contained.error() );
+    }
+
+    nsel_REQUIRES_0(
+        std::is_move_constructible<E>::value
+    )
+    nsel_constexpr14 expected( expected && other ) noexcept
+    (
+        true    // TBD - see also non-void specialization
+    )
+    : has_value_( other.has_value_ )
+    {
+        if ( ! has_value() ) contained.construct_error( std::move( other.contained.error() ) );
+    }
+
+    constexpr explicit expected( nonstd_lite_in_place_t(void) )
+    : has_value_( true )
+    {
+    }
+
+    template< typename G = E >
+    nsel_constexpr14 explicit expected( nonstd::unexpected_type<G> const & error
+        nsel_REQUIRES_A(
+            !std::is_convertible<const G&, E>::value /*=> explicit */
+        )
+    )
+    : has_value_( false )
+    {
+        contained.construct_error( error.value() );
+    }
+
+    template< typename G = E >
+    nsel_constexpr14 /*non-explicit*/ expected( nonstd::unexpected_type<G> const & error
+        nsel_REQUIRES_A(
+            std::is_convertible<const G&, E>::value /*=> non-explicit */
+        )
+    )
+    : has_value_( false )
+    {
+        contained.construct_error( error.value() );
+    }
+
+    template< typename G = E >
+    nsel_constexpr14 explicit expected( nonstd::unexpected_type<G> && error
+        nsel_REQUIRES_A(
+            !std::is_convertible<G&&, E>::value /*=> explicit */
+        )
+    )
+    : has_value_( false )
+    {
+        contained.construct_error( std::move( error.value() ) );
+    }
+
+    template< typename G = E >
+    nsel_constexpr14 /*non-explicit*/ expected( nonstd::unexpected_type<G> && error
+        nsel_REQUIRES_A(
+            std::is_convertible<G&&, E>::value /*=> non-explicit */
+        )
+    )
+    : has_value_( false )
+    {
+        contained.construct_error( std::move( error.value() ) );
+    }
+
+    template< typename... Args
+        nsel_REQUIRES_T(
+            std::is_constructible<E, Args&&...>::value
+        )
+    >
+    nsel_constexpr14 explicit expected( unexpect_t, Args&&... args )
+    : has_value_( false )
+    {
+        contained.construct_error( std::forward<Args>( args )... );
+    }
+
+    template< typename U, typename... Args
+        nsel_REQUIRES_T(
+            std::is_constructible<U, std::initializer_list<U>, Args&&...>::value
+        )
+    >
+    nsel_constexpr14 explicit expected( unexpect_t, std::initializer_list<U> il, Args&&... args )
+    : has_value_( false )
+    {
+        contained.construct_error( il, std::forward<Args>( args )... );
+    }
+
+
+    // destructor
+
+    ~expected()
+    {
+        if ( ! has_value() ) contained.destruct_error();
+    }
+
+    // x.x.4.3 assignment
+
+//    nsel_REQUIRES_A(
+//        std::is_copy_constructible<E>::value &&
+//        std::is_copy_assignable<E>::value )
+
+    expected & operator=( expected const & other )
+    {
+        expected( other ).swap( *this );
+        return *this;
+    }
+
+//    nsel_REQUIRES_A(
+//        std::is_move_constructible<E>::value &&
+//        std::is_move_assignable<E>::value )
+
+    expected & operator=( expected && other ) noexcept
+    (
+        std::is_nothrow_move_assignable<E>::value &&
+        std::is_nothrow_move_constructible<E>::value )
+    {
+        expected( std::move( other ) ).swap( *this );
+        return *this;
+    }
+
+    void emplace()
+    {}
+
+    // x.x.4.4 swap
+
+//    nsel_REQUIRES_A(
+//        std::is_move_constructible<E>::value )
+
+    void swap( expected & other ) noexcept
+    (
+#if nsel_CPP17_OR_GREATER
+        std::is_nothrow_move_constructible<E>::value && std::is_nothrow_swappable<E&>::value
+#else
+        std::is_nothrow_move_constructible<E>::value && noexcept ( std::swap( std::declval<E&>(), std::declval<E&>() ) )
+#endif
+    )
+    {
+        using std::swap;
+
+        if      ( ! bool(*this) && ! bool(other) ) { swap( contained.error(), other.contained.error() ); }
+        else if (   bool(*this) && ! bool(other) ) { contained.construct_error( std::move( other.error() ) );
+                                                   swap( has_value_, other.has_value_ ); }
+        else if ( ! bool(*this) &&   bool(other) ) { other.swap( *this ); }
+    }
+
+    // x.x.4.5 observers
+
+    constexpr explicit operator bool() const noexcept
+    {
+        return has_value();
+    }
+
+    constexpr bool has_value() const noexcept
+    {
+        return has_value_;
+    }
+
+    void value() const
+    {}
+
+    constexpr error_type const & error() const &
+    {
+        return assert( ! has_value() ), contained.error();
+    }
+
+    error_type & error() &
+    {
+        return assert( ! has_value() ), contained.error();
+    }
+
+    constexpr error_type const && error() const &&
+    {
+        return assert( ! has_value() ), std::move( contained.error() );
+    }
+
+    error_type && error() &&
+    {
+        return assert( ! has_value() ), std::move( contained.error() );
+    }
+
+    constexpr unexpected_type get_unexpected() const
+    {
+        return make_unexpected( contained.error() );
+    }
+
+    template< typename Ex >
+    bool has_exception() const
+    {
+        return ! has_value() && std::is_base_of< Ex, decltype( get_unexpected().value() ) >::value;
+    }
+
+//  template constexpr 'see below' unwrap() const&;
+//
+//  template 'see below' unwrap() &&;
+
+    // factories
+
+//  template< typename Ex, typename F>
+//  expected<void,E> catch_exception(F&& f);
+//
+//  template< typename F>
+//  expected<decltype(func()), E> map(F&& func) ;
+//
+//  template< typename F>
+//  'see below' bind(F&& func) ;
+//
+//  template< typename F>
+//  expected<void,E> catch_error(F&& f);
+//
+//  template< typename F>
+//  'see below' then(F&& func);
+
+private:
+    bool has_value_;
+    detail::storage_t<void,E> contained;
+};
+
+// expected: relational operators
+
+template< typename T, typename E >
+constexpr bool operator==( expected<T,E> const & x, expected<T,E> const & y )
+{
+    return bool(x) != bool(y) ? false : bool(x) == false ? true : *x == *y;
+}
+
+template< typename T, typename E >
+constexpr bool operator!=( expected<T,E> const & x, expected<T,E> const & y )
+{
+    return !(x == y);
+}
+
+template< typename T, typename E >
+constexpr bool operator<( expected<T,E> const & x, expected<T,E> const & y )
+{
+    return (!y) ? false : (!x) ? true : *x < *y;
+}
+
+template< typename T, typename E >
+constexpr bool operator>( expected<T,E> const & x, expected<T,E> const & y )
+{
+    return (y < x);
+}
+
+template< typename T, typename E >
+constexpr bool operator<=( expected<T,E> const & x, expected<T,E> const & y )
+{
+    return !(y < x);
+}
+
+template< typename T, typename E >
+constexpr bool operator>=( expected<T,E> const & x, expected<T,E> const & y )
+{
+    return !(x < y);
+}
+
+// expected: comparison with unexpected_type
+
+template< typename T, typename E >
+constexpr bool operator==( expected<T,E> const & x, unexpected_type<E> const & u )
+{
+    return (!x) ? x.get_unexpected() == u : false;
+}
+
+template< typename T, typename E >
+constexpr bool operator==( unexpected_type<E> const & u, expected<T,E> const & x )
+{
+    return ( x == u );
+}
+
+template< typename T, typename E >
+constexpr bool operator!=( expected<T,E> const & x, unexpected_type<E> const & u )
+{
+    return ! ( x == u );
+}
+
+template< typename T, typename E >
+constexpr bool operator!=( unexpected_type<E> const & u, expected<T,E> const & x )
+{
+    return ! ( x == u );
+}
+
+template< typename T, typename E >
+constexpr bool operator<( expected<T,E> const & x, unexpected_type<E> const & u )
+{
+    return (!x) ? ( x.get_unexpected() < u ) : false;
+}
+
+#if nsel_P0323R <= 2
+
+template< typename T, typename E >
+constexpr bool operator<( unexpected_type<E> const & u, expected<T,E> const & x )
+{
+  return (!x) ? ( u < x.get_unexpected() ) : true ;
+}
+
+template< typename T, typename E >
+constexpr bool operator>( expected<T,E> const & x, unexpected_type<E> const & u )
+{
+    return ( u < x );
+}
+
+template< typename T, typename E >
+constexpr bool operator>( unexpected_type<E> const & u, expected<T,E> const & x )
+{
+    return ( x < u );
+}
+
+template< typename T, typename E >
+constexpr bool operator<=( expected<T,E> const & x, unexpected_type<E> const & u )
+{
+    return ! ( u < x );
+}
+
+template< typename T, typename E >
+constexpr bool operator<=( unexpected_type<E> const & u, expected<T,E> const & x)
+{
+    return ! ( x < u );
+}
+
+template< typename T, typename E >
+constexpr bool operator>=( expected<T,E> const & x, unexpected_type<E> const & u  )
+{
+    return ! ( u > x );
+}
+
+template< typename T, typename E >
+constexpr bool operator>=( unexpected_type<E> const & u, expected<T,E> const & x )
+{
+    return ! ( x > u );
+}
+
+#endif // nsel_P0323R
+
+// expected: comparison with T
+
+template< typename T, typename E >
+constexpr bool operator==( expected<T,E> const & x, T const & v )
+{
+    return bool(x) ? *x == v : false;
+}
+
+template< typename T, typename E >
+constexpr bool operator==(T const & v, expected<T,E> const & x )
+{
+    return bool(x) ? v == *x : false;
+}
+
+template< typename T, typename E >
+constexpr bool operator!=( expected<T,E> const & x, T const & v )
+{
+    return bool(x) ? *x != v : true;
+}
+
+template< typename T, typename E >
+constexpr bool operator!=( T const & v, expected<T,E> const & x )
+{
+    return bool(x) ? v != *x : true;
+}
+
+template< typename T, typename E >
+constexpr bool operator<( expected<T,E> const & x, T const & v )
+{
+    return bool(x) ? *x < v : true;
+}
+
+template< typename T, typename E >
+constexpr bool operator<( T const & v, expected<T,E> const & x )
+{
+    return bool(x) ? v < *x : false;
+}
+
+template< typename T, typename E >
+constexpr bool operator>( T const & v, expected<T,E> const & x )
+{
+    return bool(x) ? *x < v : false;
+}
+
+template< typename T, typename E >
+constexpr bool operator>( expected<T,E> const & x, T const & v )
+{
+    return bool(x) ? v < *x : false;
+}
+
+template< typename T, typename E >
+constexpr bool operator<=( T const & v, expected<T,E> const & x )
+{
+    return bool(x) ? ! ( *x < v ) : false;
+}
+
+template< typename T, typename E >
+constexpr bool operator<=( expected<T,E> const & x, T const & v )
+{
+    return bool(x) ? ! ( v < *x ) : true;
+}
+
+template< typename T, typename E >
+constexpr bool operator>=( expected<T,E> const & x, T const & v )
+{
+    return bool(x) ? ! ( *x < v ) : false;
+}
+
+template< typename T, typename E >
+constexpr bool operator>=( T const & v, expected<T,E> const & x )
+{
+    return bool(x) ? ! ( v < *x ) : true;
+}
+
+/// x.x.x Specialized algorithms
+
+template< typename T, typename E >
+void swap( expected<T,E> & x, expected<T,E> & y ) noexcept ( noexcept ( x.swap(y) ) )
+{
+    x.swap( y );
+}
+
+#if nsel_P0323R <= 3
+
+template< typename T >
+constexpr auto make_expected( T && v ) -> expected< typename std::decay<T>::type >
+{
+    return expected< typename std::decay<T>::type >( std::forward<T>( v ) );
+}
+
+// expected<void> specialization:
+
+auto inline make_expected() -> expected<void>
+{
+    return expected<void>( in_place );
+}
+
+template< typename T >
+constexpr auto make_expected_from_current_exception() -> expected<T>
+{
+    return expected<T>( make_unexpected_from_current_exception() );
+}
+
+template< typename T >
+auto make_expected_from_exception( std::exception_ptr v ) -> expected<T>
+{
+    return expected<T>( unexpected_type<std::exception_ptr>( std::forward<std::exception_ptr>( v ) ) );
+}
+
+template< typename T, typename E >
+constexpr auto make_expected_from_error( E e ) -> expected<T, typename std::decay<E>::type>
+{
+    return expected<T, typename std::decay<E>::type>( make_unexpected( e ) );
+}
+
+template< typename F >
+/*nsel_constexpr14*/
+auto make_expected_from_call( F f,
+    nsel_REQUIRES_A( ! std::is_same<typename std::result_of<F()>::type, void>::value )
+) -> expected< typename std::result_of<F()>::type >
+{
+    try
+    {
+        return make_expected( f() );
+    }
+    catch (...)
+    {
+        return make_unexpected_from_current_exception();
+    }
+}
+
+template< typename F >
+/*nsel_constexpr14*/
+auto make_expected_from_call( F f,
+    nsel_REQUIRES_A( std::is_same<typename std::result_of<F()>::type, void>::value )
+) -> expected<void>
+{
+    try
+    {
+        f();
+        return make_expected();
+    }
+    catch (...)
+    {
+        return make_unexpected_from_current_exception();
+    }
+}
+
+#endif // nsel_P0323R
+
+} // namespace expected_lite
+
+using namespace expected_lite;
+
+// using expected_lite::expected;
+// using ...
+
+} // namespace nonstd
+
+namespace std {
+
+// expected: hash support
+
+template< typename T, typename E >
+struct hash< nonstd::expected<T,E> >
+{
+    using result_type = typename hash<T>::result_type;
+    using argument_type = nonstd::expected<T,E>;
+
+    constexpr result_type operator()(argument_type const & arg) const
+    {
+        return arg ? std::hash<T>{}(*arg) : result_type{};
+    }
+};
+
+// TBD - ?? remove? see spec.
+template< typename T, typename E >
+struct hash< nonstd::expected<T&,E> >
+{
+    using result_type = typename hash<T>::result_type;
+    using argument_type = nonstd::expected<T&,E>;
+
+    constexpr result_type operator()(argument_type const & arg) const
+    {
+        return arg ? std::hash<T>{}(*arg) : result_type{};
+    }
+};
+
+// TBD - implement
+// bool(e), hash<expected<void,E>>()(e) shall evaluate to the hashing true;
+// otherwise it evaluates to an unspecified value if E is exception_ptr or
+// a combination of hashing false and hash<E>()(e.error()).
+
+template< typename E >
+struct hash< nonstd::expected<void,E> >
+{
+};
+
+} // namespace std
+
+namespace nonstd {
+
+// void unexpected() is deprecated && removed in C++17
+
+#if nsel_CPP17_OR_GREATER && nsel_COMPILER_MSVC_VERSION > 141
+template< typename E >
+using unexpected = unexpected_type<E>;
+#endif
+
+} // namespace nonstd
+
+#undef nsel_REQUIRES
+#undef nsel_REQUIRES_0
+#undef nsel_REQUIRES_T
+
+nsel_RESTORE_WARNINGS()
+
+#endif // nsel_USES_STD_EXPECTED
+
+#endif // NONSTD_EXPECTED_LITE_HPP
diff --git a/include/behaviortree_cpp_v3/utils/make_unique.hpp b/include/behaviortree_cpp_v3/utils/make_unique.hpp
new file mode 100644 (file)
index 0000000..175974e
--- /dev/null
@@ -0,0 +1,53 @@
+#pragma once
+
+#include <memory>
+
+#if defined(_MSC_VER) && _MSC_VER >= 1900 // MSVC 2015 or newer.
+#  define MAKE_UNIQUE_DEFINED 1
+#endif
+
+#ifdef __cpp_lib_make_unique
+#  define MAKE_UNIQUE_DEFINED 1
+#endif
+
+#ifndef MAKE_UNIQUE_DEFINED
+
+//The compiler doesn't provide it, so implement it ourselves.
+
+#include <cstddef>
+#include <memory>
+#include <type_traits>
+#include <utility>
+
+namespace std {
+
+template<class _Ty> struct _Unique_if {
+    typedef unique_ptr<_Ty> _Single_object;
+};
+
+template<class _Ty> struct _Unique_if<_Ty[]> {
+    typedef unique_ptr<_Ty[]> _Unknown_bound;
+};
+
+template<class _Ty, size_t N> struct _Unique_if<_Ty[N]> {
+    typedef void _Known_bound;
+};
+
+template<class _Ty, class... Args>
+typename _Unique_if<_Ty>::_Single_object
+make_unique(Args&&... args) {
+    return unique_ptr<_Ty>(new _Ty(std::forward<Args>(args)...));
+}
+
+template<class _Ty>
+typename _Unique_if<_Ty>::_Unknown_bound
+make_unique(size_t n) {
+    typedef typename remove_extent<_Ty>::type U;
+    return unique_ptr<_Ty>(new U[n]());
+}
+
+
+} // namespace std
+
+#endif // !COMPILER_SUPPORTS_MAKE_UNIQUE
+
diff --git a/include/behaviortree_cpp_v3/utils/platform.hpp b/include/behaviortree_cpp_v3/utils/platform.hpp
new file mode 100644 (file)
index 0000000..a36c8aa
--- /dev/null
@@ -0,0 +1,119 @@
+//
+// Platform.h
+//
+// $Id: //poco/1.3/Foundation/include/Poco/Platform.h#5 $
+//
+// Library: Foundation
+// Package: Core
+// Module:  Platform
+//
+// Platform and architecture identification macros.
+//
+// NOTE: This file may be included from both C++ and C code, so it
+//       must not contain any C++ specific things.
+//
+// Copyright (c) 2004-2006, Applied Informatics Software Engineering GmbH.
+// and Contributors.
+//
+// Permission is hereby granted, free of charge, to any person or organization
+// obtaining a copy of the software and accompanying documentation covered by
+// this license (the "Software") to use, reproduce, display, distribute,
+// execute, and transmit the Software, and to prepare derivative works of the
+// Software, and to permit third-parties to whom the Software is furnished to
+// do so, all subject to the following:
+//
+// The copyright notices in the Software and this entire statement, including
+// the above license grant, this restriction and the following disclaimer,
+// must be included in all copies of the Software, in whole or in part, and
+// all derivative works of the Software, unless such copies or derivative
+// works are solely in the form of machine-executable object code generated by
+// a source language processor.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
+// SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
+// FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
+// ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+// DEALINGS IN THE SOFTWARE.
+//
+
+#ifndef Foundation_Platform_INCLUDED
+#define Foundation_Platform_INCLUDED
+
+//
+// Platform Identification
+//
+#define BT_OS_FREE_BSD 0x0001
+#define BT_OS_AIX 0x0002
+#define BT_OS_HPUX 0x0003
+#define BT_OS_TRU64 0x0004
+#define BT_OS_LINUX 0x0005
+#define BT_OS_MAC_OS_X 0x0006
+#define BT_OS_NET_BSD 0x0007
+#define BT_OS_OPEN_BSD 0x0008
+#define BT_OS_IRIX 0x0009
+#define BT_OS_SOLARIS 0x000a
+#define BT_OS_QNX 0x000b
+#define BT_OS_VXWORKS 0x000c
+#define BT_OS_CYGWIN 0x000d
+#define BT_OS_UNKNOWN_UNIX 0x00ff
+#define BT_OS_WINDOWS_NT 0x1001
+#define BT_OS_WINDOWS_CE 0x1011
+#define BT_OS_VMS 0x2001
+
+#if defined(__FreeBSD__) || defined(__FreeBSD_kernel__)
+#define BT_OS_FAMILY_UNIX 1
+#define BT_OS_FAMILY_BSD 1
+#define BT_OS BT_OS_FREE_BSD
+#elif defined(_AIX) || defined(__TOS_AIX__)
+#define BT_OS_FAMILY_UNIX 1
+#define BT_OS BT_OS_AIX
+#elif defined(hpux) || defined(_hpux)
+#define BT_OS_FAMILY_UNIX 1
+#define BT_OS BT_OS_HPUX
+#elif defined(__digital__) || defined(__osf__)
+#define BT_OS_FAMILY_UNIX 1
+#define BT_OS BT_OS_TRU64
+#elif defined(linux) || defined(__linux) || defined(__linux__) || defined(__TOS_LINUX__)
+#define BT_OS_FAMILY_UNIX 1
+#define BT_OS BT_OS_LINUX
+#elif defined(__APPLE__) || defined(__TOS_MACOS__)
+#define BT_OS_FAMILY_UNIX 1
+#define BT_OS_FAMILY_BSD 1
+#define BT_OS BT_OS_MAC_OS_X
+#elif defined(__NetBSD__)
+#define BT_OS_FAMILY_UNIX 1
+#define BT_OS_FAMILY_BSD 1
+#define BT_OS BT_OS_NET_BSD
+#elif defined(__OpenBSD__)
+#define BT_OS_FAMILY_UNIX 1
+#define BT_OS_FAMILY_BSD 1
+#define BT_OS BT_OS_OPEN_BSD
+#elif defined(sgi) || defined(__sgi)
+#define BT_OS_FAMILY_UNIX 1
+#define BT_OS BT_OS_IRIX
+#elif defined(sun) || defined(__sun)
+#define BT_OS_FAMILY_UNIX 1
+#define BT_OS BT_OS_SOLARIS
+#elif defined(__QNX__)
+#define BT_OS_FAMILY_UNIX 1
+#define BT_OS BT_OS_QNX
+#elif defined(unix) || defined(__unix) || defined(__unix__)
+#define BT_OS_FAMILY_UNIX 1
+#define BT_OS BT_OS_UNKNOWN_UNIX
+#elif defined(_WIN32_WCE)
+#define BT_OS_FAMILY_WINDOWS 1
+#define BT_OS BT_OS_WINDOWS_CE
+#elif defined(_WIN32) || defined(_WIN64)
+#define BT_OS_FAMILY_WINDOWS 1
+#define BT_OS BT_OS_WINDOWS_NT
+#elif defined(__CYGWIN__)
+#define BT_OS_FAMILY_UNIX 1
+#define BT_OS BT_OS_CYGWIN
+#elif defined(__VMS)
+#define BT_OS_FAMILY_VMS 1
+#define BT_OS BT_OS_VMS
+#endif
+
+#endif   // Foundation_Platform_INCLUDED
diff --git a/include/behaviortree_cpp_v3/utils/safe_any.hpp b/include/behaviortree_cpp_v3/utils/safe_any.hpp
new file mode 100644 (file)
index 0000000..7f9fe5b
--- /dev/null
@@ -0,0 +1,249 @@
+#ifndef SAFE_ANY_VARNUMBER_H
+#define SAFE_ANY_VARNUMBER_H
+
+#include <exception>
+#include <algorithm>
+#include <iostream>
+#include <chrono>
+#include <string>
+#include <cstring>
+#include <type_traits>
+#include "any.hpp"
+#include "demangle_util.h"
+#include "convert_impl.hpp"
+#include "expected.hpp"
+#include "strcat.hpp"
+#include "strcat.hpp"
+
+namespace BT
+{
+// Rational: since type erased numbers will always use at least 8 bytes
+// it is faster to cast everything to either double, uint64_t or int64_t.
+class Any
+{
+    template <typename T>
+    using EnableIntegral =
+        typename std::enable_if<std::is_integral<T>::value || std::is_enum<T>::value>::type*;
+
+    template <typename T>
+    using EnableNonIntegral =
+        typename std::enable_if<!std::is_integral<T>::value && !std::is_enum<T>::value>::type*;
+
+    template <typename T>
+    using EnableString = typename std::enable_if<std::is_same<T, std::string>::value>::type*;
+
+    template <typename T>
+    using EnableArithmetic = typename std::enable_if<std::is_arithmetic<T>::value>::type*;
+
+    template <typename T>
+    using EnableEnum = typename std::enable_if<std::is_enum<T>::value>::type*;
+
+    template <typename T>
+    using EnableUnknownType =
+        typename std::enable_if<!std::is_arithmetic<T>::value && !std::is_enum<T>::value &&
+                                !std::is_same<T, std::string>::value>::type*;
+
+  public:
+    Any(): _original_type(nullptr)
+    {
+    }
+
+    ~Any() = default;
+
+    Any(const Any& other) : _any(other._any), _original_type( other._original_type )
+    {
+    }
+
+    Any(Any&& other) : _any( std::move(other._any) ), _original_type( other._original_type )
+    {
+    }
+
+    explicit Any(const double& value) : _any(value), _original_type( &typeid(double) )
+    {
+    }
+
+    explicit Any(const uint64_t& value) : _any(value), _original_type( &typeid(uint64_t) )
+    {
+    }
+
+    explicit Any(const float& value) : _any(double(value)), _original_type( &typeid(float) )
+    {
+    }
+
+    explicit Any(const std::string& str) : _any(SafeAny::SimpleString(str)), _original_type( &typeid(std::string) )
+    {
+    }
+
+    explicit Any(const char* str) : _any(SafeAny::SimpleString(str)), _original_type( &typeid(std::string) )
+    {
+    }
+
+    explicit Any(const SafeAny::SimpleString& str) : _any(str), _original_type( &typeid(std::string) )
+    {
+    }
+
+    // all the other integrals are casted to int64_t
+    template <typename T>
+    explicit Any(const T& value, EnableIntegral<T> = 0) : _any(int64_t(value)), _original_type( &typeid(T) )
+    {
+    }
+
+    // default for other custom types
+    template <typename T>
+    explicit Any(const T& value, EnableNonIntegral<T> = 0) : _any(value), _original_type( &typeid(T) )
+    {
+    }
+
+    Any& operator = (const Any& other)
+    {
+        this->_any = other._any;
+        this->_original_type = other._original_type;
+        return *this;
+    }
+
+    bool isNumber() const
+    {
+        return _any.type() == typeid(int64_t) ||
+               _any.type() == typeid(uint64_t) ||
+               _any.type() == typeid(double);
+    }
+
+    bool isString() const
+    {
+        return _any.type() == typeid(SafeAny::SimpleString);
+    }
+
+    // this is different from any_cast, because if allows safe
+    // conversions between arithmetic values.
+    template <typename T>
+    T cast() const
+    {
+        if( _any.empty() )
+        {
+            throw std::runtime_error("Any::cast failed because it is empty");
+        }
+        if (_any.type() == typeid(T))
+        {
+            return linb::any_cast<T>(_any);
+        }
+        else
+        {
+            auto res = convert<T>();
+            if( !res )
+            {
+                throw std::runtime_error( res.error() );
+            }
+            return res.value();
+        }
+    }
+
+    const std::type_info& type() const noexcept
+    {
+        return *_original_type;
+    }
+
+    const std::type_info& castedType() const noexcept
+    {
+        return _any.type();
+    }
+
+    bool empty() const noexcept
+    {
+        return _any.empty();
+    }
+
+  private:
+    linb::any _any;
+    const std::type_info* _original_type;
+
+    //----------------------------
+
+    template <typename DST>
+    nonstd::expected<DST,std::string> convert(EnableString<DST> = 0) const
+    {
+        const auto& type = _any.type();
+
+        if (type == typeid(SafeAny::SimpleString))
+        {
+            return linb::any_cast<SafeAny::SimpleString>(_any).toStdString();
+        }
+        else if (type == typeid(int64_t))
+        {
+            return std::to_string(linb::any_cast<int64_t>(_any));
+        }
+        else if (type == typeid(uint64_t))
+        {
+            return std::to_string(linb::any_cast<uint64_t>(_any));
+        }
+        else if (type == typeid(double))
+        {
+            return std::to_string(linb::any_cast<double>(_any));
+        }
+
+        return nonstd::make_unexpected( errorMsg<DST>() );
+    }
+
+    template <typename DST>
+    nonstd::expected<DST,std::string> convert(EnableArithmetic<DST> = 0) const
+    {
+        using SafeAny::details::convertNumber;
+        DST out;
+
+        const auto& type = _any.type();
+
+        if (type == typeid(int64_t))
+        {
+            convertNumber<int64_t, DST>(linb::any_cast<int64_t>(_any), out);
+        }
+        else if (type == typeid(uint64_t))
+        {
+            convertNumber<uint64_t, DST>(linb::any_cast<uint64_t>(_any), out);
+        }
+        else if (type == typeid(double))
+        {
+            convertNumber<double, DST>(linb::any_cast<double>(_any), out);
+        }
+        else{
+            return nonstd::make_unexpected( errorMsg<DST>() );
+        }
+        return out;
+    }
+
+    template <typename DST>
+    nonstd::expected<DST,std::string> convert(EnableEnum<DST> = 0) const
+    {
+        using SafeAny::details::convertNumber;
+
+        const auto& type = _any.type();
+
+        if (type == typeid(int64_t))
+        {
+            uint64_t out = linb::any_cast<int64_t>(_any);
+            return static_cast<DST>(out);
+        }
+        else if (type == typeid(uint64_t))
+        {
+            uint64_t out = linb::any_cast<uint64_t>(_any);
+            return static_cast<DST>(out);
+        }
+
+        return nonstd::make_unexpected( errorMsg<DST>() );
+    }
+
+    template <typename DST>
+    nonstd::expected<DST,std::string> convert(EnableUnknownType<DST> = 0) const
+    {
+        return nonstd::make_unexpected( errorMsg<DST>() );
+    }
+
+    template <typename T>
+    std::string errorMsg() const
+    {
+        return StrCat("[Any::convert]: no known safe conversion between [",
+                      demangle( _any.type() ), "] and [", demangle( typeid(T) ),"]");
+    }
+};
+
+}   // end namespace BT
+
+#endif   // VARNUMBER_H
diff --git a/include/behaviortree_cpp_v3/utils/shared_library.h b/include/behaviortree_cpp_v3/utils/shared_library.h
new file mode 100644 (file)
index 0000000..bd3cfa2
--- /dev/null
@@ -0,0 +1,143 @@
+//
+// SharedLibrary.h
+//
+// $Id: //poco/1.3/Foundation/include/Poco/SharedLibrary.h#1 $
+//
+// Library: Foundation
+// Package: SharedLibrary
+// Module:  SharedLibrary
+//
+// Definition of the SharedLibrary class.
+//
+// Copyright (c) 2004-2006, Applied Informatics Software Engineering GmbH.
+// and Contributors.
+//
+// Permission is hereby granted, free of charge, to any person or organization
+// obtaining a copy of the software and accompanying documentation covered by
+// this license (the "Software") to use, reproduce, display, distribute,
+// execute, and transmit the Software, and to prepare derivative works of the
+// Software, and to permit third-parties to whom the Software is furnished to
+// do so, all subject to the following:
+//
+// The copyright notices in the Software and this entire statement, including
+// the above license grant, this restriction and the following disclaimer,
+// must be included in all copies of the Software, in whole or in part, and
+// all derivative works of the Software, unless such copies or derivative
+// works are solely in the form of machine-executable object code generated by
+// a source language processor.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
+// SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
+// FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
+// ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+// DEALINGS IN THE SOFTWARE.
+//
+
+#ifndef Foundation_SharedLibrary_INCLUDED
+#define Foundation_SharedLibrary_INCLUDED
+
+#include "platform.hpp"
+#include <string>
+#include <mutex>
+
+namespace BT
+{
+class SharedLibrary
+/// The SharedLibrary class dynamically
+/// loads shared libraries at run-time.
+{
+  public:
+    enum Flags
+    {
+        SHLIB_GLOBAL = 1,
+        /// On platforms that use dlopen(), use RTLD_GLOBAL. This is the default
+        /// if no flags are given.
+        ///
+        /// This flag is ignored on platforms that do not use dlopen().
+
+        SHLIB_LOCAL = 2
+        /// On platforms that use dlopen(), use RTLD_LOCAL instead of RTLD_GLOBAL.
+        ///
+        /// Note that if this flag is specified, RTTI (including dynamic_cast and throw) will
+        /// not work for types defined in the shared library with GCC and possibly other
+        /// compilers as well. See http://gcc.gnu.org/faq.html#dso for more information.
+        ///
+        /// This flag is ignored on platforms that do not use dlopen().
+    };
+
+    SharedLibrary();
+    /// Creates a SharedLibrary object.
+
+    SharedLibrary(const std::string& path, int flags = 0);
+    /// Creates a SharedLibrary object and loads a library
+    /// from the given path, using the given flags.
+    /// See the Flags enumeration for valid values.
+
+    virtual ~SharedLibrary() = default;
+    /// Destroys the SharedLibrary. The actual library
+    /// remains loaded.
+
+    void load(const std::string& path, int flags = 0);
+    /// Loads a shared library from the given path,
+    /// using the given flags. See the Flags enumeration
+    /// for valid values.
+    /// Throws a LibraryAlreadyLoadedException if
+    /// a library has already been loaded.
+    /// Throws a LibraryLoadException if the library
+    /// cannot be loaded.
+
+    void unload();
+    /// Unloads a shared library.
+
+    bool isLoaded() const;
+    /// Returns true iff a library has been loaded.
+
+    bool hasSymbol(const std::string& name);
+    /// Returns true iff the loaded library contains
+    /// a symbol with the given name.
+
+    void* getSymbol(const std::string& name);
+    /// Returns the address of the symbol with
+    /// the given name. For functions, this
+    /// is the entry point of the function.
+    /// Throws a NotFoundException if the symbol
+    /// does not exist.
+
+    const std::string& getPath() const;
+    /// Returns the path of the library, as
+    /// specified in a call to load() or the
+    /// constructor.
+
+    static std::string prefix();
+    /// Returns the platform-specific filename prefix
+    /// for shared libraries.
+    /// Most platforms would return "lib" as prefix, while
+    /// on Cygwin, the "cyg" prefix will be returned.
+
+    static std::string suffix();
+    /// Returns the platform-specific filename suffix
+    /// for shared libraries (including the period).
+    /// In debug mode, the suffix also includes a
+    /// "d" to specify the debug version of a library.
+
+    static std::string getOSName(const std::string& name);
+    /// Returns the platform-specific filename
+    /// for shared libraries by prefixing and suffixing name
+    /// with prefix() and suffix()
+
+  private:
+    SharedLibrary(const SharedLibrary&);
+    SharedLibrary& operator=(const SharedLibrary&);
+
+    void* findSymbol(const std::string& name);
+
+    std::string _path;
+    void* _handle;
+    std::mutex _mutex;
+};
+
+}   // namespace Poco
+
+#endif   // Foundation_SharedLibrary_INCLUDED
diff --git a/include/behaviortree_cpp_v3/utils/signal.h b/include/behaviortree_cpp_v3/utils/signal.h
new file mode 100644 (file)
index 0000000..9bcb467
--- /dev/null
@@ -0,0 +1,49 @@
+#ifndef SIMPLE_SIGNAL_H
+#define SIMPLE_SIGNAL_H
+
+#include <memory>
+#include <functional>
+#include <vector>
+
+namespace BT
+{
+/**
+ * Super simple Signal/Slop implementation, AKA "Observable pattern".
+ * The subscriber is active until it goes out of scope or Subscriber::reset() is called.
+ */
+template <typename... CallableArgs>
+class Signal
+{
+  public:
+    using CallableFunction = std::function<void(CallableArgs...)>;
+    using Subscriber = std::shared_ptr<CallableFunction>;
+
+    void notify(CallableArgs... args)
+    {
+        for (size_t i = 0; i < subscribers_.size();)
+        {
+            if (auto sub = subscribers_[i].lock())
+            {
+                (*sub)(args...);
+                i++;
+            }
+            else
+            {
+                subscribers_.erase(subscribers_.begin() + i);
+            }
+        }
+    }
+
+    Subscriber subscribe(CallableFunction func)
+    {
+        Subscriber sub = std::make_shared<CallableFunction>(std::move(func));
+        subscribers_.emplace_back(sub);
+        return sub;
+    }
+
+  private:
+    std::vector<std::weak_ptr<CallableFunction>> subscribers_;
+};
+}
+
+#endif   // SIMPLE_SIGNAL_H
diff --git a/include/behaviortree_cpp_v3/utils/simple_string.hpp b/include/behaviortree_cpp_v3/utils/simple_string.hpp
new file mode 100644 (file)
index 0000000..1bab0c4
--- /dev/null
@@ -0,0 +1,92 @@
+#ifndef SIMPLE_STRING_HPP
+#define SIMPLE_STRING_HPP
+
+#include <string>
+#include <cstring>
+
+namespace SafeAny
+{
+// Version of string that uses only two words. Good for small object optimization in linb::any
+class SimpleString
+{
+  public:
+    SimpleString(const std::string& str) : SimpleString(str.data(), str.size())
+    {
+    }
+    SimpleString(const char* input_data) : SimpleString(input_data, strlen(input_data))
+    {
+    }
+
+    SimpleString(const char* input_data, std::size_t size) : _size(size)
+    {
+        if(size >= sizeof(void*) )
+        {
+            _data.ptr = new char[_size + 1];
+        }
+        std::memcpy(data(), input_data, _size);
+        data()[_size] = '\0';
+    }
+
+    SimpleString(const SimpleString& other) : SimpleString(other.data(), other.size())
+    {
+    }
+
+    SimpleString& operator = (const SimpleString& other)
+    {
+      _data = other._data;
+      _size = other._size;
+      return *this;
+    }
+
+    ~SimpleString()
+    {
+        if ( _size >= sizeof(void*) && _data.ptr )
+        {
+            delete[] _data.ptr;
+        }
+    }
+
+    std::string toStdString() const
+    {
+        return std::string(data(), _size);
+    }
+
+    const char* data() const
+    {
+        if( _size >= sizeof(void*))
+        {
+            return _data.ptr;
+        }
+        else{
+            return _data.soo;
+        }
+    }
+
+    char* data()
+    {
+        if( _size >= sizeof(void*))
+        {
+            return _data.ptr;
+        }
+        else{
+            return _data.soo;
+        }
+    }
+
+    std::size_t size() const
+    {
+        return _size;
+    }
+
+  private:
+    union{
+        char*  ptr;
+        char   soo[sizeof(void*)] ;
+    }_data;
+
+    std::size_t _size;
+};
+
+}
+
+#endif   // SIMPLE_STRING_HPP
diff --git a/include/behaviortree_cpp_v3/utils/strcat.hpp b/include/behaviortree_cpp_v3/utils/strcat.hpp
new file mode 100644 (file)
index 0000000..e41c878
--- /dev/null
@@ -0,0 +1,117 @@
+#ifndef STRCAT_HPP
+#define STRCAT_HPP
+
+#include "string_view.hpp"
+#include <array>
+#include <cstdint>
+#include <string>
+#include <type_traits>
+
+namespace BT {
+
+// -----------------------------------------------------------------------------
+// StrCat()
+// -----------------------------------------------------------------------------
+//
+// Merges given strings, using no delimiter(s).
+//
+// `StrCat()` is designed to be the fastest possible way to construct a string
+// out of a mix of raw C strings, string_views, strings.
+
+namespace strings_internal {
+
+inline void AppendPieces(std::string* dest,
+                         std::initializer_list<nonstd::string_view> pieces)
+{
+    size_t size = 0;
+    for (const auto& piece: pieces)
+    {
+        size += piece.size();
+    }
+    dest->reserve(dest->size() + size);
+    for (const auto& piece: pieces)
+    {
+        dest->append( piece.data(), piece.size() );
+    }
+}
+
+inline std::string CatPieces(std::initializer_list<nonstd::string_view> pieces)
+{
+    std::string out;
+    AppendPieces(&out, std::move(pieces));
+    return out;
+}
+
+}  // namespace strings_internal
+
+inline std::string StrCat() { return std::string(); }
+
+inline std::string StrCat(const nonstd::string_view& a) {
+    return std::string(a.data(), a.size());
+}
+
+inline std::string StrCat(const nonstd::string_view& a,
+                   const nonstd::string_view& b)
+{
+    return strings_internal::CatPieces( {a, b} );
+}
+
+inline std::string StrCat(const nonstd::string_view& a,
+                          const nonstd::string_view& b,
+                          const nonstd::string_view& c)
+{
+    return strings_internal::CatPieces( {a, b, c} );
+}
+
+// Support 4 or more arguments
+template <typename... AV>
+inline std::string StrCat(const nonstd::string_view& a,
+                          const nonstd::string_view& b,
+                          const nonstd::string_view& c,
+                          const nonstd::string_view& d,
+                          const AV&... args)
+{
+    return strings_internal::CatPieces( {a, b, c, d,  static_cast<const nonstd::string_view&>(args)...});
+}
+
+//-----------------------------------------------
+
+
+inline void StrAppend(std::string* destination,
+                      const nonstd::string_view& a)
+{
+    destination->append( a.data(), a.size());
+}
+
+inline void StrAppend(std::string* destination,
+                      const nonstd::string_view& a,
+                      const nonstd::string_view& b)
+{
+    strings_internal::AppendPieces( destination, {a, b} );
+}
+
+inline void StrAppend(std::string* destination,
+                      const nonstd::string_view& a,
+                      const nonstd::string_view& b,
+                      const nonstd::string_view& c)
+{
+    strings_internal::AppendPieces( destination, {a, b, c} );
+}
+
+// Support 4 or more arguments
+template <typename... AV>
+inline void StrAppend(std::string* destination,
+                      const nonstd::string_view& a,
+                      const nonstd::string_view& b,
+                      const nonstd::string_view& c,
+                      const nonstd::string_view& d,
+                      const AV&... args)
+{
+    strings_internal::AppendPieces( destination, {a, b, c, d,  static_cast<const nonstd::string_view&>(args)...});
+}
+
+
+}  // namespace BT
+
+
+#endif // STRCAT_HPP
diff --git a/include/behaviortree_cpp_v3/utils/string_view.hpp b/include/behaviortree_cpp_v3/utils/string_view.hpp
new file mode 100644 (file)
index 0000000..aa52d94
--- /dev/null
@@ -0,0 +1,1532 @@
+// Copyright 2017-2019 by Martin Moene
+//
+// string-view lite, a C++17-like string_view for C++98 and later.
+// For more information see https://github.com/martinmoene/string-view-lite
+//
+// Distributed under the Boost Software License, Version 1.0.
+// (See accompanying file LICENSE.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
+
+#pragma once
+
+#ifndef NONSTD_SV_LITE_H_INCLUDED
+#define NONSTD_SV_LITE_H_INCLUDED
+
+#define string_view_lite_MAJOR  1
+#define string_view_lite_MINOR  4
+#define string_view_lite_PATCH  0
+
+#define string_view_lite_VERSION  nssv_STRINGIFY(string_view_lite_MAJOR) "." nssv_STRINGIFY(string_view_lite_MINOR) "." nssv_STRINGIFY(string_view_lite_PATCH)
+
+#define nssv_STRINGIFY(  x )  nssv_STRINGIFY_( x )
+#define nssv_STRINGIFY_( x )  #x
+
+// string-view lite configuration:
+
+#define nssv_STRING_VIEW_DEFAULT  0
+#define nssv_STRING_VIEW_NONSTD   1
+#define nssv_STRING_VIEW_STD      2
+
+// forced by BehaviorTree.CPP
+#define nssv_CONFIG_SELECT_STRING_VIEW nssv_STRING_VIEW_NONSTD
+
+//#if !defined( nssv_CONFIG_SELECT_STRING_VIEW )
+//# define nssv_CONFIG_SELECT_STRING_VIEW  ( nssv_HAVE_STD_STRING_VIEW ? nssv_STRING_VIEW_STD : nssv_STRING_VIEW_NONSTD )
+//#endif
+
+#if defined( nssv_CONFIG_SELECT_STD_STRING_VIEW ) || defined( nssv_CONFIG_SELECT_NONSTD_STRING_VIEW )
+# error nssv_CONFIG_SELECT_STD_STRING_VIEW and nssv_CONFIG_SELECT_NONSTD_STRING_VIEW are deprecated and removed, please use nssv_CONFIG_SELECT_STRING_VIEW=nssv_STRING_VIEW_...
+#endif
+
+#ifndef  nssv_CONFIG_STD_SV_OPERATOR
+# define nssv_CONFIG_STD_SV_OPERATOR  0
+#endif
+
+#ifndef  nssv_CONFIG_USR_SV_OPERATOR
+# define nssv_CONFIG_USR_SV_OPERATOR  1
+#endif
+
+#ifdef   nssv_CONFIG_CONVERSION_STD_STRING
+# define nssv_CONFIG_CONVERSION_STD_STRING_CLASS_METHODS   nssv_CONFIG_CONVERSION_STD_STRING
+# define nssv_CONFIG_CONVERSION_STD_STRING_FREE_FUNCTIONS  nssv_CONFIG_CONVERSION_STD_STRING
+#endif
+
+#ifndef  nssv_CONFIG_CONVERSION_STD_STRING_CLASS_METHODS
+# define nssv_CONFIG_CONVERSION_STD_STRING_CLASS_METHODS  1
+#endif
+
+#ifndef  nssv_CONFIG_CONVERSION_STD_STRING_FREE_FUNCTIONS
+# define nssv_CONFIG_CONVERSION_STD_STRING_FREE_FUNCTIONS  1
+#endif
+
+// Control presence of exception handling (try and auto discover):
+
+#ifndef nssv_CONFIG_NO_EXCEPTIONS
+# if defined(__cpp_exceptions) || defined(__EXCEPTIONS) || defined(_CPPUNWIND)
+#  define nssv_CONFIG_NO_EXCEPTIONS  0
+# else
+#  define nssv_CONFIG_NO_EXCEPTIONS  1
+# endif
+#endif
+
+// C++ language version detection (C++20 is speculative):
+// Note: VC14.0/1900 (VS2015) lacks too much from C++14.
+
+#ifndef   nssv_CPLUSPLUS
+# if defined(_MSVC_LANG ) && !defined(__clang__)
+#  define nssv_CPLUSPLUS  (_MSC_VER == 1900 ? 201103L : _MSVC_LANG )
+# else
+#  define nssv_CPLUSPLUS  __cplusplus
+# endif
+#endif
+
+#define nssv_CPP98_OR_GREATER  ( nssv_CPLUSPLUS >= 199711L )
+#define nssv_CPP11_OR_GREATER  ( nssv_CPLUSPLUS >= 201103L )
+#define nssv_CPP11_OR_GREATER_ ( nssv_CPLUSPLUS >= 201103L )
+#define nssv_CPP14_OR_GREATER  ( nssv_CPLUSPLUS >= 201402L )
+#define nssv_CPP17_OR_GREATER  ( nssv_CPLUSPLUS >= 201703L )
+#define nssv_CPP20_OR_GREATER  ( nssv_CPLUSPLUS >= 202000L )
+
+// use C++17 std::string_view if available and requested:
+
+#if nssv_CPP17_OR_GREATER && defined(__has_include )
+# if __has_include( <string_view> )
+#  define nssv_HAVE_STD_STRING_VIEW  1
+# else
+#  define nssv_HAVE_STD_STRING_VIEW  0
+# endif
+#else
+# define  nssv_HAVE_STD_STRING_VIEW  0
+#endif
+
+#define  nssv_USES_STD_STRING_VIEW  ( (nssv_CONFIG_SELECT_STRING_VIEW == nssv_STRING_VIEW_STD) || ((nssv_CONFIG_SELECT_STRING_VIEW == nssv_STRING_VIEW_DEFAULT) && nssv_HAVE_STD_STRING_VIEW) )
+
+#define nssv_HAVE_STARTS_WITH ( nssv_CPP20_OR_GREATER || !nssv_USES_STD_STRING_VIEW )
+#define nssv_HAVE_ENDS_WITH     nssv_HAVE_STARTS_WITH
+
+//
+// Use C++17 std::string_view:
+//
+
+#if nssv_USES_STD_STRING_VIEW
+
+#include <string_view>
+
+// Extensions for std::string:
+
+#if nssv_CONFIG_CONVERSION_STD_STRING_FREE_FUNCTIONS
+
+namespace nonstd {
+
+template< class CharT, class Traits, class Allocator = std::allocator<CharT> >
+std::basic_string<CharT, Traits, Allocator>
+to_string( std::basic_string_view<CharT, Traits> v, Allocator const & a = Allocator() )
+{
+  return std::basic_string<CharT,Traits, Allocator>( v.begin(), v.end(), a );
+}
+
+template< class CharT, class Traits, class Allocator >
+std::basic_string_view<CharT, Traits>
+to_string_view( std::basic_string<CharT, Traits, Allocator> const & s )
+{
+  return std::basic_string_view<CharT, Traits>( s.data(), s.size() );
+}
+
+// Literal operators sv and _sv:
+
+#if nssv_CONFIG_STD_SV_OPERATOR
+
+using namespace std::literals::string_view_literals;
+
+#endif
+
+#if nssv_CONFIG_USR_SV_OPERATOR
+
+inline namespace literals {
+inline namespace string_view_literals {
+
+
+constexpr std::string_view operator "" _sv( const char* str, size_t len ) noexcept  // (1)
+{
+  return std::string_view{ str, len };
+}
+
+constexpr std::u16string_view operator "" _sv( const char16_t* str, size_t len ) noexcept  // (2)
+{
+  return std::u16string_view{ str, len };
+}
+
+constexpr std::u32string_view operator "" _sv( const char32_t* str, size_t len ) noexcept  // (3)
+{
+  return std::u32string_view{ str, len };
+}
+
+constexpr std::wstring_view operator "" _sv( const wchar_t* str, size_t len ) noexcept  // (4)
+{
+  return std::wstring_view{ str, len };
+}
+
+}} // namespace literals::string_view_literals
+
+#endif // nssv_CONFIG_USR_SV_OPERATOR
+
+} // namespace nonstd
+
+#endif // nssv_CONFIG_CONVERSION_STD_STRING_FREE_FUNCTIONS
+
+namespace nonstd {
+
+using std::string_view;
+using std::wstring_view;
+using std::u16string_view;
+using std::u32string_view;
+using std::basic_string_view;
+
+// literal "sv" and "_sv", see above
+
+using std::operator==;
+using std::operator!=;
+using std::operator<;
+using std::operator<=;
+using std::operator>;
+using std::operator>=;
+
+using std::operator<<;
+
+} // namespace nonstd
+
+#else // nssv_HAVE_STD_STRING_VIEW
+
+//
+// Before C++17: use string_view lite:
+//
+
+// Compiler versions:
+//
+// MSVC++  6.0  _MSC_VER == 1200  nssv_COMPILER_MSVC_VERSION ==  60  (Visual Studio 6.0)
+// MSVC++  7.0  _MSC_VER == 1300  nssv_COMPILER_MSVC_VERSION ==  70  (Visual Studio .NET 2002)
+// MSVC++  7.1  _MSC_VER == 1310  nssv_COMPILER_MSVC_VERSION ==  71  (Visual Studio .NET 2003)
+// MSVC++  8.0  _MSC_VER == 1400  nssv_COMPILER_MSVC_VERSION ==  80  (Visual Studio 2005)
+// MSVC++  9.0  _MSC_VER == 1500  nssv_COMPILER_MSVC_VERSION ==  90  (Visual Studio 2008)
+// MSVC++ 10.0  _MSC_VER == 1600  nssv_COMPILER_MSVC_VERSION == 100  (Visual Studio 2010)
+// MSVC++ 11.0  _MSC_VER == 1700  nssv_COMPILER_MSVC_VERSION == 110  (Visual Studio 2012)
+// MSVC++ 12.0  _MSC_VER == 1800  nssv_COMPILER_MSVC_VERSION == 120  (Visual Studio 2013)
+// MSVC++ 14.0  _MSC_VER == 1900  nssv_COMPILER_MSVC_VERSION == 140  (Visual Studio 2015)
+// MSVC++ 14.1  _MSC_VER >= 1910  nssv_COMPILER_MSVC_VERSION == 141  (Visual Studio 2017)
+// MSVC++ 14.2  _MSC_VER >= 1920  nssv_COMPILER_MSVC_VERSION == 142  (Visual Studio 2019)
+
+#if defined(_MSC_VER ) && !defined(__clang__)
+# define nssv_COMPILER_MSVC_VER      (_MSC_VER )
+# define nssv_COMPILER_MSVC_VERSION  (_MSC_VER / 10 - 10 * ( 5 + (_MSC_VER < 1900 ) ) )
+#else
+# define nssv_COMPILER_MSVC_VER      0
+# define nssv_COMPILER_MSVC_VERSION  0
+#endif
+
+#define nssv_COMPILER_VERSION( major, minor, patch )  ( 10 * ( 10 * (major) + (minor) ) + (patch) )
+
+#if defined(__clang__)
+# define nssv_COMPILER_CLANG_VERSION  nssv_COMPILER_VERSION(__clang_major__, __clang_minor__, __clang_patchlevel__)
+#else
+# define nssv_COMPILER_CLANG_VERSION    0
+#endif
+
+#if defined(__GNUC__) && !defined(__clang__)
+# define nssv_COMPILER_GNUC_VERSION  nssv_COMPILER_VERSION(__GNUC__, __GNUC_MINOR__, __GNUC_PATCHLEVEL__)
+#else
+# define nssv_COMPILER_GNUC_VERSION    0
+#endif
+
+// half-open range [lo..hi):
+#define nssv_BETWEEN( v, lo, hi ) ( (lo) <= (v) && (v) < (hi) )
+
+// Presence of language and library features:
+
+#ifdef _HAS_CPP0X
+# define nssv_HAS_CPP0X  _HAS_CPP0X
+#else
+# define nssv_HAS_CPP0X  0
+#endif
+
+// Unless defined otherwise below, consider VC14 as C++11 for variant-lite:
+
+#if nssv_COMPILER_MSVC_VER >= 1900
+# undef  nssv_CPP11_OR_GREATER
+# define nssv_CPP11_OR_GREATER  1
+#endif
+
+#define nssv_CPP11_90   (nssv_CPP11_OR_GREATER_ || nssv_COMPILER_MSVC_VER >= 1500)
+#define nssv_CPP11_100  (nssv_CPP11_OR_GREATER_ || nssv_COMPILER_MSVC_VER >= 1600)
+#define nssv_CPP11_110  (nssv_CPP11_OR_GREATER_ || nssv_COMPILER_MSVC_VER >= 1700)
+#define nssv_CPP11_120  (nssv_CPP11_OR_GREATER_ || nssv_COMPILER_MSVC_VER >= 1800)
+#define nssv_CPP11_140  (nssv_CPP11_OR_GREATER_ || nssv_COMPILER_MSVC_VER >= 1900)
+#define nssv_CPP11_141  (nssv_CPP11_OR_GREATER_ || nssv_COMPILER_MSVC_VER >= 1910)
+
+#define nssv_CPP14_000  (nssv_CPP14_OR_GREATER)
+#define nssv_CPP17_000  (nssv_CPP17_OR_GREATER)
+
+// Presence of C++11 language features:
+
+#define nssv_HAVE_CONSTEXPR_11          nssv_CPP11_140
+#define nssv_HAVE_EXPLICIT_CONVERSION   nssv_CPP11_140
+#define nssv_HAVE_INLINE_NAMESPACE      nssv_CPP11_140
+#define nssv_HAVE_NOEXCEPT              nssv_CPP11_140
+#define nssv_HAVE_NULLPTR               nssv_CPP11_100
+#define nssv_HAVE_REF_QUALIFIER         nssv_CPP11_140
+#define nssv_HAVE_UNICODE_LITERALS      nssv_CPP11_140
+#define nssv_HAVE_USER_DEFINED_LITERALS nssv_CPP11_140
+#define nssv_HAVE_WCHAR16_T             nssv_CPP11_100
+#define nssv_HAVE_WCHAR32_T             nssv_CPP11_100
+
+#if ! ( ( nssv_CPP11_OR_GREATER && nssv_COMPILER_CLANG_VERSION ) || nssv_BETWEEN( nssv_COMPILER_CLANG_VERSION, 300, 400 ) )
+# define nssv_HAVE_STD_DEFINED_LITERALS  nssv_CPP11_140
+#else
+# define nssv_HAVE_STD_DEFINED_LITERALS  0
+#endif
+
+// Presence of C++14 language features:
+
+#define nssv_HAVE_CONSTEXPR_14          nssv_CPP14_000
+
+// Presence of C++17 language features:
+
+#define nssv_HAVE_NODISCARD             nssv_CPP17_000
+
+// Presence of C++ library features:
+
+#define nssv_HAVE_STD_HASH              nssv_CPP11_120
+
+// C++ feature usage:
+
+#if nssv_HAVE_CONSTEXPR_11
+# define nssv_constexpr  constexpr
+#else
+# define nssv_constexpr  /*constexpr*/
+#endif
+
+#if  nssv_HAVE_CONSTEXPR_14
+# define nssv_constexpr14  constexpr
+#else
+# define nssv_constexpr14  /*constexpr*/
+#endif
+
+#if nssv_HAVE_EXPLICIT_CONVERSION
+# define nssv_explicit  explicit
+#else
+# define nssv_explicit  /*explicit*/
+#endif
+
+#if nssv_HAVE_INLINE_NAMESPACE
+# define nssv_inline_ns  inline
+#else
+# define nssv_inline_ns  /*inline*/
+#endif
+
+#if nssv_HAVE_NOEXCEPT
+# define nssv_noexcept  noexcept
+#else
+# define nssv_noexcept  /*noexcept*/
+#endif
+
+//#if nssv_HAVE_REF_QUALIFIER
+//# define nssv_ref_qual  &
+//# define nssv_refref_qual  &&
+//#else
+//# define nssv_ref_qual  /*&*/
+//# define nssv_refref_qual  /*&&*/
+//#endif
+
+#if nssv_HAVE_NULLPTR
+# define nssv_nullptr  nullptr
+#else
+# define nssv_nullptr  NULL
+#endif
+
+#if nssv_HAVE_NODISCARD
+# define nssv_nodiscard  [[nodiscard]]
+#else
+# define nssv_nodiscard  /*[[nodiscard]]*/
+#endif
+
+// Additional includes:
+
+#include <algorithm>
+#include <cassert>
+#include <iterator>
+#include <limits>
+#include <ostream>
+#include <string>   // std::char_traits<>
+
+#if ! nssv_CONFIG_NO_EXCEPTIONS
+# include <stdexcept>
+#endif
+
+#if nssv_CPP11_OR_GREATER
+# include <type_traits>
+#endif
+
+// Clang, GNUC, MSVC warning suppression macros:
+
+#if defined(__clang__)
+# pragma clang diagnostic ignored "-Wreserved-user-defined-literal"
+# pragma clang diagnostic push
+# pragma clang diagnostic ignored "-Wuser-defined-literals"
+#elif defined(__GNUC__)
+# pragma  GCC  diagnostic push
+# pragma  GCC  diagnostic ignored "-Wliteral-suffix"
+#endif // __clang__
+
+#if nssv_COMPILER_MSVC_VERSION >= 140
+# define nssv_SUPPRESS_MSGSL_WARNING(expr)        [[gsl::suppress(expr)]]
+# define nssv_SUPPRESS_MSVC_WARNING(code, descr)  __pragma(warning(suppress: code) )
+# define nssv_DISABLE_MSVC_WARNINGS(codes)        __pragma(warning(push))  __pragma(warning(disable: codes))
+#else
+# define nssv_SUPPRESS_MSGSL_WARNING(expr)
+# define nssv_SUPPRESS_MSVC_WARNING(code, descr)
+# define nssv_DISABLE_MSVC_WARNINGS(codes)
+#endif
+
+#if defined(__clang__)
+# define nssv_RESTORE_WARNINGS()  _Pragma("clang diagnostic pop")
+#elif defined(__GNUC__)
+# define nssv_RESTORE_WARNINGS()  _Pragma("GCC diagnostic pop")
+#elif nssv_COMPILER_MSVC_VERSION >= 140
+# define nssv_RESTORE_WARNINGS()  __pragma(warning(pop ))
+#else
+# define nssv_RESTORE_WARNINGS()
+#endif
+
+// Suppress the following MSVC (GSL) warnings:
+// - C4455, non-gsl   : 'operator ""sv': literal suffix identifiers that do not
+//                      start with an underscore are reserved
+// - C26472, gsl::t.1 : don't use a static_cast for arithmetic conversions;
+//                      use brace initialization, gsl::narrow_cast or gsl::narow
+// - C26481: gsl::b.1 : don't use pointer arithmetic. Use span instead
+
+nssv_DISABLE_MSVC_WARNINGS( 4455 26481 26472 )
+  //nssv_DISABLE_CLANG_WARNINGS( "-Wuser-defined-literals" )
+  //nssv_DISABLE_GNUC_WARNINGS( -Wliteral-suffix )
+
+  namespace nonstd { namespace sv_lite {
+
+#if nssv_CPP11_OR_GREATER
+
+  namespace detail {
+
+#if nssv_CPP14_OR_GREATER
+
+  template< typename CharT >
+  inline constexpr std::size_t length( CharT * s, std::size_t result = 0 )
+  {
+    CharT * v = s;
+    std::size_t r = result;
+    while ( *v != '\0' ) {
+      ++v;
+      ++r;
+    }
+    return r;
+  }
+
+#else // nssv_CPP14_OR_GREATER
+
+  // Expect tail call optimization to make length() non-recursive:
+
+  template< typename CharT >
+  inline constexpr std::size_t length( CharT * s, std::size_t result = 0 )
+  {
+    return *s == '\0' ? result : length( s + 1, result + 1 );
+  }
+
+#endif // nssv_CPP14_OR_GREATER
+
+  } // namespace detail
+
+#endif // nssv_CPP11_OR_GREATER
+
+  template
+    <
+      class CharT,
+      class Traits = std::char_traits<CharT>
+      >
+    class basic_string_view;
+
+  //
+  // basic_string_view:
+  //
+
+  template
+    <
+      class CharT,
+      class Traits /* = std::char_traits<CharT> */
+      >
+    class basic_string_view
+  {
+  public:
+    // Member types:
+
+    typedef Traits traits_type;
+    typedef CharT  value_type;
+
+    typedef CharT       * pointer;
+    typedef CharT const * const_pointer;
+    typedef CharT       & reference;
+    typedef CharT const & const_reference;
+
+    typedef const_pointer iterator;
+    typedef const_pointer const_iterator;
+    typedef std::reverse_iterator< const_iterator > reverse_iterator;
+    typedef    std::reverse_iterator< const_iterator > const_reverse_iterator;
+
+    typedef std::size_t     size_type;
+    typedef std::ptrdiff_t  difference_type;
+
+    // 24.4.2.1 Construction and assignment:
+
+    nssv_constexpr basic_string_view() nssv_noexcept
+    : data_( nssv_nullptr )
+    , size_( 0 )
+    {}
+
+#if nssv_CPP11_OR_GREATER
+    nssv_constexpr basic_string_view( basic_string_view const & other ) nssv_noexcept = default;
+#else
+    nssv_constexpr basic_string_view( basic_string_view const & other ) nssv_noexcept
+    : data_( other.data_)
+    , size_( other.size_)
+    {}
+#endif
+
+    nssv_constexpr basic_string_view( CharT const * s, size_type count ) nssv_noexcept // non-standard noexcept
+    : data_( s )
+    , size_( count )
+    {}
+
+    nssv_constexpr basic_string_view( CharT const * s) nssv_noexcept // non-standard noexcept
+    : data_( s )
+#if nssv_CPP17_OR_GREATER
+    , size_( Traits::length(s) )
+#elif nssv_CPP11_OR_GREATER
+    , size_( detail::length(s) )
+#else
+    , size_( Traits::length(s) )
+#endif
+    {}
+
+    // Assignment:
+
+#if nssv_CPP11_OR_GREATER
+    nssv_constexpr14 basic_string_view & operator=( basic_string_view const & other ) nssv_noexcept = default;
+#else
+    nssv_constexpr14 basic_string_view & operator=( basic_string_view const & other ) nssv_noexcept
+    {
+      data_ = other.data_;
+      size_ = other.size_;
+      return *this;
+    }
+#endif
+
+    // 24.4.2.2 Iterator support:
+
+    nssv_constexpr const_iterator begin()  const nssv_noexcept { return data_;         }
+    nssv_constexpr const_iterator end()    const nssv_noexcept { return data_ + size_; }
+
+    nssv_constexpr const_iterator cbegin() const nssv_noexcept { return begin(); }
+    nssv_constexpr const_iterator cend()   const nssv_noexcept { return end();   }
+
+    nssv_constexpr const_reverse_iterator rbegin()  const nssv_noexcept { return const_reverse_iterator( end() );   }
+    nssv_constexpr const_reverse_iterator rend()    const nssv_noexcept { return const_reverse_iterator( begin() ); }
+
+    nssv_constexpr const_reverse_iterator crbegin() const nssv_noexcept { return rbegin(); }
+    nssv_constexpr const_reverse_iterator crend()   const nssv_noexcept { return rend();   }
+
+    // 24.4.2.3 Capacity:
+
+    nssv_constexpr size_type size()     const nssv_noexcept { return size_; }
+    nssv_constexpr size_type length()   const nssv_noexcept { return size_; }
+    nssv_constexpr size_type max_size() const nssv_noexcept { return (std::numeric_limits< size_type >::max)(); }
+
+    // since C++20
+    nssv_nodiscard nssv_constexpr bool empty() const nssv_noexcept
+    {
+      return 0 == size_;
+    }
+
+    // 24.4.2.4 Element access:
+
+    nssv_constexpr const_reference operator[]( size_type pos ) const
+    {
+      return data_at( pos );
+    }
+
+    nssv_constexpr14 const_reference at( size_type pos ) const
+    {
+#if nssv_CONFIG_NO_EXCEPTIONS
+      assert( pos < size() );
+#else
+      if ( pos >= size() )
+      {
+        throw std::out_of_range("nonstd::string_view::at()");
+      }
+#endif
+      return data_at( pos );
+    }
+
+    nssv_constexpr const_reference front() const { return data_at( 0 );          }
+    nssv_constexpr const_reference back()  const { return data_at( size() - 1 ); }
+
+    nssv_constexpr const_pointer   data()  const nssv_noexcept { return data_; }
+
+    // 24.4.2.5 Modifiers:
+
+    nssv_constexpr14 void remove_prefix( size_type n )
+    {
+      assert( n <= size() );
+      data_ += n;
+      size_ -= n;
+    }
+
+    nssv_constexpr14 void remove_suffix( size_type n )
+    {
+      assert( n <= size() );
+      size_ -= n;
+    }
+
+    nssv_constexpr14 void swap( basic_string_view & other ) nssv_noexcept
+    {
+      using std::swap;
+      swap( data_, other.data_ );
+      swap( size_, other.size_ );
+    }
+
+    // 24.4.2.6 String operations:
+
+    size_type copy( CharT * dest, size_type n, size_type pos = 0 ) const
+    {
+#if nssv_CONFIG_NO_EXCEPTIONS
+      assert( pos <= size() );
+#else
+      if ( pos > size() )
+      {
+        throw std::out_of_range("nonstd::string_view::copy()");
+      }
+#endif
+      const size_type rlen = (std::min)( n, size() - pos );
+
+      (void) Traits::copy( dest, data() + pos, rlen );
+
+      return rlen;
+    }
+
+    nssv_constexpr14 basic_string_view substr( size_type pos = 0, size_type n = npos ) const
+    {
+#if nssv_CONFIG_NO_EXCEPTIONS
+      assert( pos <= size() );
+#else
+      if ( pos > size() )
+      {
+        throw std::out_of_range("nonstd::string_view::substr()");
+      }
+#endif
+      return basic_string_view( data() + pos, (std::min)( n, size() - pos ) );
+    }
+
+    // compare(), 6x:
+
+    nssv_constexpr14 int compare( basic_string_view other ) const nssv_noexcept // (1)
+    {
+      if ( const int result = Traits::compare( data(), other.data(), (std::min)( size(), other.size() ) ) )
+      {
+        return result;
+      }
+
+      return size() == other.size() ? 0 : size() < other.size() ? -1 : 1;
+    }
+
+    nssv_constexpr int compare( size_type pos1, size_type n1, basic_string_view other ) const // (2)
+    {
+      return substr( pos1, n1 ).compare( other );
+    }
+
+    nssv_constexpr int compare( size_type pos1, size_type n1, basic_string_view other, size_type pos2, size_type n2 ) const // (3)
+    {
+      return substr( pos1, n1 ).compare( other.substr( pos2, n2 ) );
+    }
+
+    nssv_constexpr int compare( CharT const * s ) const // (4)
+    {
+      return compare( basic_string_view( s ) );
+    }
+
+    nssv_constexpr int compare( size_type pos1, size_type n1, CharT const * s ) const // (5)
+    {
+      return substr( pos1, n1 ).compare( basic_string_view( s ) );
+    }
+
+    nssv_constexpr int compare( size_type pos1, size_type n1, CharT const * s, size_type n2 ) const // (6)
+    {
+      return substr( pos1, n1 ).compare( basic_string_view( s, n2 ) );
+    }
+
+    // 24.4.2.7 Searching:
+
+    // starts_with(), 3x, since C++20:
+
+    nssv_constexpr bool starts_with( basic_string_view v ) const nssv_noexcept  // (1)
+    {
+      return size() >= v.size() && compare( 0, v.size(), v ) == 0;
+    }
+
+    nssv_constexpr bool starts_with( CharT c ) const nssv_noexcept  // (2)
+    {
+      return starts_with( basic_string_view( &c, 1 ) );
+    }
+
+    nssv_constexpr bool starts_with( CharT const * s ) const  // (3)
+    {
+      return starts_with( basic_string_view( s ) );
+    }
+
+    // ends_with(), 3x, since C++20:
+
+    nssv_constexpr bool ends_with( basic_string_view v ) const nssv_noexcept  // (1)
+    {
+      return size() >= v.size() && compare( size() - v.size(), npos, v ) == 0;
+    }
+
+    nssv_constexpr bool ends_with( CharT c ) const nssv_noexcept  // (2)
+    {
+      return ends_with( basic_string_view( &c, 1 ) );
+    }
+
+    nssv_constexpr bool ends_with( CharT const * s ) const  // (3)
+    {
+      return ends_with( basic_string_view( s ) );
+    }
+
+    // find(), 4x:
+
+    nssv_constexpr14 size_type find( basic_string_view v, size_type pos = 0 ) const nssv_noexcept  // (1)
+    {
+      return assert( v.size() == 0 || v.data() != nssv_nullptr )
+               , pos >= size()
+               ? npos
+               : to_pos( std::search( cbegin() + pos, cend(), v.cbegin(), v.cend(), Traits::eq ) );
+    }
+
+    nssv_constexpr14 size_type find( CharT c, size_type pos = 0 ) const nssv_noexcept  // (2)
+    {
+      return find( basic_string_view( &c, 1 ), pos );
+    }
+
+    nssv_constexpr14 size_type find( CharT const * s, size_type pos, size_type n ) const  // (3)
+    {
+      return find( basic_string_view( s, n ), pos );
+    }
+
+    nssv_constexpr14 size_type find( CharT const * s, size_type pos = 0 ) const  // (4)
+    {
+      return find( basic_string_view( s ), pos );
+    }
+
+    // rfind(), 4x:
+
+    nssv_constexpr14 size_type rfind( basic_string_view v, size_type pos = npos ) const nssv_noexcept  // (1)
+    {
+      if ( size() < v.size() )
+      {
+        return npos;
+      }
+
+      if ( v.empty() )
+      {
+        return (std::min)( size(), pos );
+      }
+
+      const_iterator last   = cbegin() + (std::min)( size() - v.size(), pos ) + v.size();
+      const_iterator result = std::find_end( cbegin(), last, v.cbegin(), v.cend(), Traits::eq );
+
+      return result != last ? size_type( result - cbegin() ) : npos;
+    }
+
+    nssv_constexpr14 size_type rfind( CharT c, size_type pos = npos ) const nssv_noexcept  // (2)
+    {
+      return rfind( basic_string_view( &c, 1 ), pos );
+    }
+
+    nssv_constexpr14 size_type rfind( CharT const * s, size_type pos, size_type n ) const  // (3)
+    {
+      return rfind( basic_string_view( s, n ), pos );
+    }
+
+    nssv_constexpr14 size_type rfind( CharT const * s, size_type pos = npos ) const  // (4)
+    {
+      return rfind( basic_string_view( s ), pos );
+    }
+
+    // find_first_of(), 4x:
+
+    nssv_constexpr size_type find_first_of( basic_string_view v, size_type pos = 0 ) const nssv_noexcept  // (1)
+    {
+      return pos >= size()
+               ? npos
+               : to_pos( std::find_first_of( cbegin() + pos, cend(), v.cbegin(), v.cend(), Traits::eq ) );
+    }
+
+    nssv_constexpr size_type find_first_of( CharT c, size_type pos = 0 ) const nssv_noexcept  // (2)
+    {
+      return find_first_of( basic_string_view( &c, 1 ), pos );
+    }
+
+    nssv_constexpr size_type find_first_of( CharT const * s, size_type pos, size_type n ) const  // (3)
+    {
+      return find_first_of( basic_string_view( s, n ), pos );
+    }
+
+    nssv_constexpr size_type find_first_of(  CharT const * s, size_type pos = 0 ) const  // (4)
+    {
+      return find_first_of( basic_string_view( s ), pos );
+    }
+
+    // find_last_of(), 4x:
+
+    nssv_constexpr size_type find_last_of( basic_string_view v, size_type pos = npos ) const nssv_noexcept  // (1)
+    {
+      return empty()
+               ? npos
+               : pos >= size()
+                   ? find_last_of( v, size() - 1 )
+                   : to_pos( std::find_first_of( const_reverse_iterator( cbegin() + pos + 1 ), crend(), v.cbegin(), v.cend(), Traits::eq ) );
+    }
+
+    nssv_constexpr size_type find_last_of( CharT c, size_type pos = npos ) const nssv_noexcept  // (2)
+    {
+      return find_last_of( basic_string_view( &c, 1 ), pos );
+    }
+
+    nssv_constexpr size_type find_last_of( CharT const * s, size_type pos, size_type count ) const  // (3)
+    {
+      return find_last_of( basic_string_view( s, count ), pos );
+    }
+
+    nssv_constexpr size_type find_last_of( CharT const * s, size_type pos = npos ) const  // (4)
+    {
+      return find_last_of( basic_string_view( s ), pos );
+    }
+
+    // find_first_not_of(), 4x:
+
+    nssv_constexpr size_type find_first_not_of( basic_string_view v, size_type pos = 0 ) const nssv_noexcept  // (1)
+    {
+      return pos >= size()
+               ? npos
+               : to_pos( std::find_if( cbegin() + pos, cend(), not_in_view( v ) ) );
+    }
+
+    nssv_constexpr size_type find_first_not_of( CharT c, size_type pos = 0 ) const nssv_noexcept  // (2)
+    {
+      return find_first_not_of( basic_string_view( &c, 1 ), pos );
+    }
+
+    nssv_constexpr size_type find_first_not_of( CharT const * s, size_type pos, size_type count ) const  // (3)
+    {
+      return find_first_not_of( basic_string_view( s, count ), pos );
+    }
+
+    nssv_constexpr size_type find_first_not_of( CharT const * s, size_type pos = 0 ) const  // (4)
+    {
+      return find_first_not_of( basic_string_view( s ), pos );
+    }
+
+    // find_last_not_of(), 4x:
+
+    nssv_constexpr size_type find_last_not_of( basic_string_view v, size_type pos = npos ) const nssv_noexcept  // (1)
+    {
+      return empty()
+               ? npos
+               : pos >= size()
+                   ? find_last_not_of( v, size() - 1 )
+                   : to_pos( std::find_if( const_reverse_iterator( cbegin() + pos + 1 ), crend(), not_in_view( v ) ) );
+    }
+
+    nssv_constexpr size_type find_last_not_of( CharT c, size_type pos = npos ) const nssv_noexcept  // (2)
+    {
+      return find_last_not_of( basic_string_view( &c, 1 ), pos );
+    }
+
+    nssv_constexpr size_type find_last_not_of( CharT const * s, size_type pos, size_type count ) const  // (3)
+    {
+      return find_last_not_of( basic_string_view( s, count ), pos );
+    }
+
+    nssv_constexpr size_type find_last_not_of( CharT const * s, size_type pos = npos ) const  // (4)
+    {
+      return find_last_not_of( basic_string_view( s ), pos );
+    }
+
+    // Constants:
+
+#if nssv_CPP17_OR_GREATER
+    static nssv_constexpr size_type npos = size_type(-1);
+#elif nssv_CPP11_OR_GREATER
+    enum : size_type { npos = size_type(-1) };
+#else
+    enum { npos = size_type(-1) };
+#endif
+
+  private:
+    struct not_in_view
+    {
+      const basic_string_view v;
+
+      nssv_constexpr explicit not_in_view( basic_string_view v ) : v( v ) {}
+
+      nssv_constexpr bool operator()( CharT c ) const
+      {
+        return npos == v.find_first_of( c );
+      }
+    };
+
+    nssv_constexpr size_type to_pos( const_iterator it ) const
+    {
+      return it == cend() ? npos : size_type( it - cbegin() );
+    }
+
+    nssv_constexpr size_type to_pos( const_reverse_iterator it ) const
+    {
+      return it == crend() ? npos : size_type( crend() - it - 1 );
+    }
+
+    nssv_constexpr const_reference data_at( size_type pos ) const
+    {
+#if nssv_BETWEEN( nssv_COMPILER_GNUC_VERSION, 1, 500 )
+      return data_[pos];
+#else
+      return assert( pos < size() ), data_[pos];
+#endif
+    }
+
+  private:
+    const_pointer data_;
+    size_type     size_;
+
+  public:
+#if nssv_CONFIG_CONVERSION_STD_STRING_CLASS_METHODS
+
+    template< class Allocator >
+    basic_string_view( std::basic_string<CharT, Traits, Allocator> const & s ) nssv_noexcept
+    : data_( s.data() )
+    , size_( s.size() )
+    {}
+
+#if nssv_HAVE_EXPLICIT_CONVERSION
+
+    template< class Allocator >
+    explicit operator std::basic_string<CharT, Traits, Allocator>() const
+    {
+      return to_string( Allocator() );
+    }
+
+#endif // nssv_HAVE_EXPLICIT_CONVERSION
+
+#if nssv_CPP11_OR_GREATER
+
+    template< class Allocator = std::allocator<CharT> >
+    std::basic_string<CharT, Traits, Allocator>
+    to_string( Allocator const & a = Allocator() ) const
+    {
+      return std::basic_string<CharT, Traits, Allocator>( begin(), end(), a );
+    }
+
+#else
+
+    std::basic_string<CharT, Traits>
+    to_string() const
+    {
+      return std::basic_string<CharT, Traits>( begin(), end() );
+    }
+
+    template< class Allocator >
+    std::basic_string<CharT, Traits, Allocator>
+    to_string( Allocator const & a ) const
+    {
+      return std::basic_string<CharT, Traits, Allocator>( begin(), end(), a );
+    }
+
+#endif // nssv_CPP11_OR_GREATER
+
+#endif // nssv_CONFIG_CONVERSION_STD_STRING_CLASS_METHODS
+  };
+
+  //
+  // Non-member functions:
+  //
+
+  // 24.4.3 Non-member comparison functions:
+  // lexicographically compare two string views (function template):
+
+  template< class CharT, class Traits >
+  nssv_constexpr bool operator== (
+    basic_string_view <CharT, Traits> lhs,
+    basic_string_view <CharT, Traits> rhs ) nssv_noexcept
+  { return lhs.compare( rhs ) == 0 ; }
+
+  template< class CharT, class Traits >
+  nssv_constexpr bool operator!= (
+    basic_string_view <CharT, Traits> lhs,
+    basic_string_view <CharT, Traits> rhs ) nssv_noexcept
+  { return lhs.compare( rhs ) != 0 ; }
+
+  template< class CharT, class Traits >
+  nssv_constexpr bool operator< (
+    basic_string_view <CharT, Traits> lhs,
+    basic_string_view <CharT, Traits> rhs ) nssv_noexcept
+  { return lhs.compare( rhs ) < 0 ; }
+
+  template< class CharT, class Traits >
+  nssv_constexpr bool operator<= (
+    basic_string_view <CharT, Traits> lhs,
+    basic_string_view <CharT, Traits> rhs ) nssv_noexcept
+  { return lhs.compare( rhs ) <= 0 ; }
+
+  template< class CharT, class Traits >
+  nssv_constexpr bool operator> (
+    basic_string_view <CharT, Traits> lhs,
+    basic_string_view <CharT, Traits> rhs ) nssv_noexcept
+  { return lhs.compare( rhs ) > 0 ; }
+
+  template< class CharT, class Traits >
+  nssv_constexpr bool operator>= (
+    basic_string_view <CharT, Traits> lhs,
+    basic_string_view <CharT, Traits> rhs ) nssv_noexcept
+  { return lhs.compare( rhs ) >= 0 ; }
+
+// Let S be basic_string_view<CharT, Traits>, and sv be an instance of S.
+// Implementations shall provide sufficient additional overloads marked
+// constexpr and noexcept so that an object t with an implicit conversion
+// to S can be compared according to Table 67.
+
+#if ! nssv_CPP11_OR_GREATER || nssv_BETWEEN( nssv_COMPILER_MSVC_VERSION, 100, 141 )
+
+  // accomodate for older compilers:
+
+  // ==
+
+  template< class CharT, class Traits>
+  nssv_constexpr bool operator==(
+    basic_string_view<CharT, Traits> lhs,
+    CharT const * rhs ) nssv_noexcept
+  { return lhs.compare( rhs ) == 0; }
+
+  template< class CharT, class Traits>
+  nssv_constexpr bool operator==(
+    CharT const * lhs,
+    basic_string_view<CharT, Traits> rhs ) nssv_noexcept
+  { return rhs.compare( lhs ) == 0; }
+
+  template< class CharT, class Traits>
+  nssv_constexpr bool operator==(
+    basic_string_view<CharT, Traits> lhs,
+    std::basic_string<CharT, Traits> rhs ) nssv_noexcept
+  { return lhs.size() == rhs.size() && lhs.compare( rhs ) == 0; }
+
+  template< class CharT, class Traits>
+  nssv_constexpr bool operator==(
+    std::basic_string<CharT, Traits> rhs,
+    basic_string_view<CharT, Traits> lhs ) nssv_noexcept
+  { return lhs.size() == rhs.size() && lhs.compare( rhs ) == 0; }
+
+  // !=
+
+  template< class CharT, class Traits>
+  nssv_constexpr bool operator!=(
+    basic_string_view<CharT, Traits> lhs,
+    char const * rhs ) nssv_noexcept
+  { return lhs.compare( rhs ) != 0; }
+
+  template< class CharT, class Traits>
+  nssv_constexpr bool operator!=(
+    char const * lhs,
+    basic_string_view<CharT, Traits> rhs ) nssv_noexcept
+  { return rhs.compare( lhs ) != 0; }
+
+  template< class CharT, class Traits>
+  nssv_constexpr bool operator!=(
+    basic_string_view<CharT, Traits> lhs,
+    std::basic_string<CharT, Traits> rhs ) nssv_noexcept
+  { return lhs.size() != rhs.size() && lhs.compare( rhs ) != 0; }
+
+  template< class CharT, class Traits>
+  nssv_constexpr bool operator!=(
+    std::basic_string<CharT, Traits> rhs,
+    basic_string_view<CharT, Traits> lhs ) nssv_noexcept
+  { return lhs.size() != rhs.size() || rhs.compare( lhs ) != 0; }
+
+  // <
+
+  template< class CharT, class Traits>
+  nssv_constexpr bool operator<(
+    basic_string_view<CharT, Traits> lhs,
+    char const * rhs ) nssv_noexcept
+  { return lhs.compare( rhs ) < 0; }
+
+  template< class CharT, class Traits>
+  nssv_constexpr bool operator<(
+    char const * lhs,
+    basic_string_view<CharT, Traits> rhs ) nssv_noexcept
+  { return rhs.compare( lhs ) > 0; }
+
+  template< class CharT, class Traits>
+  nssv_constexpr bool operator<(
+    basic_string_view<CharT, Traits> lhs,
+    std::basic_string<CharT, Traits> rhs ) nssv_noexcept
+  { return lhs.compare( rhs ) < 0; }
+
+  template< class CharT, class Traits>
+  nssv_constexpr bool operator<(
+    std::basic_string<CharT, Traits> rhs,
+    basic_string_view<CharT, Traits> lhs ) nssv_noexcept
+  { return rhs.compare( lhs ) > 0; }
+
+  // <=
+
+  template< class CharT, class Traits>
+  nssv_constexpr bool operator<=(
+    basic_string_view<CharT, Traits> lhs,
+    char const * rhs ) nssv_noexcept
+  { return lhs.compare( rhs ) <= 0; }
+
+  template< class CharT, class Traits>
+  nssv_constexpr bool operator<=(
+    char const * lhs,
+    basic_string_view<CharT, Traits> rhs ) nssv_noexcept
+  { return rhs.compare( lhs ) >= 0; }
+
+  template< class CharT, class Traits>
+  nssv_constexpr bool operator<=(
+    basic_string_view<CharT, Traits> lhs,
+    std::basic_string<CharT, Traits> rhs ) nssv_noexcept
+  { return lhs.compare( rhs ) <= 0; }
+
+  template< class CharT, class Traits>
+  nssv_constexpr bool operator<=(
+    std::basic_string<CharT, Traits> rhs,
+    basic_string_view<CharT, Traits> lhs ) nssv_noexcept
+  { return rhs.compare( lhs ) >= 0; }
+
+  // >
+
+  template< class CharT, class Traits>
+  nssv_constexpr bool operator>(
+    basic_string_view<CharT, Traits> lhs,
+    char const * rhs ) nssv_noexcept
+  { return lhs.compare( rhs ) > 0; }
+
+  template< class CharT, class Traits>
+  nssv_constexpr bool operator>(
+    char const * lhs,
+    basic_string_view<CharT, Traits> rhs ) nssv_noexcept
+  { return rhs.compare( lhs ) < 0; }
+
+  template< class CharT, class Traits>
+  nssv_constexpr bool operator>(
+    basic_string_view<CharT, Traits> lhs,
+    std::basic_string<CharT, Traits> rhs ) nssv_noexcept
+  { return lhs.compare( rhs ) > 0; }
+
+  template< class CharT, class Traits>
+  nssv_constexpr bool operator>(
+    std::basic_string<CharT, Traits> rhs,
+    basic_string_view<CharT, Traits> lhs ) nssv_noexcept
+  { return rhs.compare( lhs ) < 0; }
+
+  // >=
+
+  template< class CharT, class Traits>
+  nssv_constexpr bool operator>=(
+    basic_string_view<CharT, Traits> lhs,
+    char const * rhs ) nssv_noexcept
+  { return lhs.compare( rhs ) >= 0; }
+
+  template< class CharT, class Traits>
+  nssv_constexpr bool operator>=(
+    char const * lhs,
+    basic_string_view<CharT, Traits> rhs ) nssv_noexcept
+  { return rhs.compare( lhs ) <= 0; }
+
+  template< class CharT, class Traits>
+  nssv_constexpr bool operator>=(
+    basic_string_view<CharT, Traits> lhs,
+    std::basic_string<CharT, Traits> rhs ) nssv_noexcept
+  { return lhs.compare( rhs ) >= 0; }
+
+  template< class CharT, class Traits>
+  nssv_constexpr bool operator>=(
+    std::basic_string<CharT, Traits> rhs,
+    basic_string_view<CharT, Traits> lhs ) nssv_noexcept
+  { return rhs.compare( lhs ) <= 0; }
+
+#else // newer compilers:
+
+#define nssv_BASIC_STRING_VIEW_I(T,U)  typename std::decay< basic_string_view<T,U> >::type
+
+#if nssv_BETWEEN( nssv_COMPILER_MSVC_VERSION, 140, 150 )
+# define nssv_MSVC_ORDER(x)  , int=x
+#else
+# define nssv_MSVC_ORDER(x)  /*, int=x*/
+#endif
+
+  // ==
+
+  template< class CharT, class Traits  nssv_MSVC_ORDER(1) >
+  nssv_constexpr bool operator==(
+    basic_string_view  <CharT, Traits> lhs,
+    nssv_BASIC_STRING_VIEW_I(CharT, Traits) rhs ) nssv_noexcept
+  { return lhs.compare( rhs ) == 0; }
+
+  template< class CharT, class Traits  nssv_MSVC_ORDER(2) >
+  nssv_constexpr bool operator==(
+    nssv_BASIC_STRING_VIEW_I(CharT, Traits) lhs,
+    basic_string_view  <CharT, Traits> rhs ) nssv_noexcept
+  { return lhs.size() == rhs.size() && lhs.compare( rhs ) == 0; }
+
+  // !=
+
+  template< class CharT, class Traits  nssv_MSVC_ORDER(1) >
+  nssv_constexpr bool operator!= (
+    basic_string_view  < CharT, Traits > lhs,
+    nssv_BASIC_STRING_VIEW_I( CharT, Traits ) rhs ) nssv_noexcept
+  { return lhs.size() != rhs.size() || lhs.compare( rhs ) != 0 ; }
+
+  template< class CharT, class Traits  nssv_MSVC_ORDER(2) >
+  nssv_constexpr bool operator!= (
+    nssv_BASIC_STRING_VIEW_I( CharT, Traits ) lhs,
+    basic_string_view  < CharT, Traits > rhs ) nssv_noexcept
+  { return lhs.compare( rhs ) != 0 ; }
+
+  // <
+
+  template< class CharT, class Traits  nssv_MSVC_ORDER(1) >
+  nssv_constexpr bool operator< (
+    basic_string_view  < CharT, Traits > lhs,
+    nssv_BASIC_STRING_VIEW_I( CharT, Traits ) rhs ) nssv_noexcept
+  { return lhs.compare( rhs ) < 0 ; }
+
+  template< class CharT, class Traits  nssv_MSVC_ORDER(2) >
+  nssv_constexpr bool operator< (
+    nssv_BASIC_STRING_VIEW_I( CharT, Traits ) lhs,
+    basic_string_view  < CharT, Traits > rhs ) nssv_noexcept
+  { return lhs.compare( rhs ) < 0 ; }
+
+  // <=
+
+  template< class CharT, class Traits  nssv_MSVC_ORDER(1) >
+  nssv_constexpr bool operator<= (
+    basic_string_view  < CharT, Traits > lhs,
+    nssv_BASIC_STRING_VIEW_I( CharT, Traits ) rhs ) nssv_noexcept
+  { return lhs.compare( rhs ) <= 0 ; }
+
+  template< class CharT, class Traits  nssv_MSVC_ORDER(2) >
+  nssv_constexpr bool operator<= (
+    nssv_BASIC_STRING_VIEW_I( CharT, Traits ) lhs,
+    basic_string_view  < CharT, Traits > rhs ) nssv_noexcept
+  { return lhs.compare( rhs ) <= 0 ; }
+
+  // >
+
+  template< class CharT, class Traits  nssv_MSVC_ORDER(1) >
+  nssv_constexpr bool operator> (
+    basic_string_view  < CharT, Traits > lhs,
+    nssv_BASIC_STRING_VIEW_I( CharT, Traits ) rhs ) nssv_noexcept
+  { return lhs.compare( rhs ) > 0 ; }
+
+  template< class CharT, class Traits  nssv_MSVC_ORDER(2) >
+  nssv_constexpr bool operator> (
+    nssv_BASIC_STRING_VIEW_I( CharT, Traits ) lhs,
+    basic_string_view  < CharT, Traits > rhs ) nssv_noexcept
+  { return lhs.compare( rhs ) > 0 ; }
+
+  // >=
+
+  template< class CharT, class Traits  nssv_MSVC_ORDER(1) >
+  nssv_constexpr bool operator>= (
+    basic_string_view  < CharT, Traits > lhs,
+    nssv_BASIC_STRING_VIEW_I( CharT, Traits ) rhs ) nssv_noexcept
+  { return lhs.compare( rhs ) >= 0 ; }
+
+  template< class CharT, class Traits  nssv_MSVC_ORDER(2) >
+  nssv_constexpr bool operator>= (
+    nssv_BASIC_STRING_VIEW_I( CharT, Traits ) lhs,
+    basic_string_view  < CharT, Traits > rhs ) nssv_noexcept
+  { return lhs.compare( rhs ) >= 0 ; }
+
+#undef nssv_MSVC_ORDER
+#undef nssv_BASIC_STRING_VIEW_I
+
+#endif // compiler-dependent approach to comparisons
+
+  // 24.4.4 Inserters and extractors:
+
+  namespace detail {
+
+  template< class Stream >
+  void write_padding( Stream & os, std::streamsize n )
+  {
+    for ( std::streamsize i = 0; i < n; ++i )
+      os.rdbuf()->sputc( os.fill() );
+  }
+
+  template< class Stream, class View >
+  Stream & write_to_stream( Stream & os, View const & sv )
+  {
+    typename Stream::sentry sentry( os );
+
+    if ( !os )
+      return os;
+
+    const std::streamsize length = static_cast<std::streamsize>( sv.length() );
+
+    // Whether, and how, to pad:
+    const bool      pad = ( length < os.width() );
+    const bool left_pad = pad && ( os.flags() & std::ios_base::adjustfield ) == std::ios_base::right;
+
+    if ( left_pad )
+      write_padding( os, os.width() - length );
+
+    // Write span characters:
+    os.rdbuf()->sputn( sv.begin(), length );
+
+    if ( pad && !left_pad )
+      write_padding( os, os.width() - length );
+
+    // Reset output stream width:
+    os.width( 0 );
+
+    return os;
+  }
+
+  } // namespace detail
+
+  template< class CharT, class Traits >
+  std::basic_ostream<CharT, Traits> &
+  operator<<(
+    std::basic_ostream<CharT, Traits>& os,
+    basic_string_view <CharT, Traits> sv )
+  {
+    return detail::write_to_stream( os, sv );
+  }
+
+  // Several typedefs for common character types are provided:
+
+  typedef basic_string_view<char>      string_view;
+  typedef basic_string_view<wchar_t>   wstring_view;
+#if nssv_HAVE_WCHAR16_T
+  typedef basic_string_view<char16_t>  u16string_view;
+  typedef basic_string_view<char32_t>  u32string_view;
+#endif
+
+  }} // namespace nonstd::sv_lite
+
+//
+// 24.4.6 Suffix for basic_string_view literals:
+//
+
+#if nssv_HAVE_USER_DEFINED_LITERALS
+
+namespace nonstd {
+nssv_inline_ns namespace literals {
+  nssv_inline_ns namespace string_view_literals {
+
+#if nssv_CONFIG_STD_SV_OPERATOR && nssv_HAVE_STD_DEFINED_LITERALS
+
+    nssv_constexpr nonstd::sv_lite::string_view operator "" sv( const char* str, size_t len ) nssv_noexcept  // (1)
+    {
+      return nonstd::sv_lite::string_view{ str, len };
+    }
+
+    nssv_constexpr nonstd::sv_lite::u16string_view operator "" sv( const char16_t* str, size_t len ) nssv_noexcept  // (2)
+    {
+      return nonstd::sv_lite::u16string_view{ str, len };
+    }
+
+    nssv_constexpr nonstd::sv_lite::u32string_view operator "" sv( const char32_t* str, size_t len ) nssv_noexcept  // (3)
+    {
+      return nonstd::sv_lite::u32string_view{ str, len };
+    }
+
+    nssv_constexpr nonstd::sv_lite::wstring_view operator "" sv( const wchar_t* str, size_t len ) nssv_noexcept  // (4)
+    {
+      return nonstd::sv_lite::wstring_view{ str, len };
+    }
+
+#endif // nssv_CONFIG_STD_SV_OPERATOR && nssv_HAVE_STD_DEFINED_LITERALS
+
+#if nssv_CONFIG_USR_SV_OPERATOR
+
+    nssv_constexpr nonstd::sv_lite::string_view operator "" _sv( const char* str, size_t len ) nssv_noexcept  // (1)
+    {
+      return nonstd::sv_lite::string_view{ str, len };
+    }
+
+    nssv_constexpr nonstd::sv_lite::u16string_view operator "" _sv( const char16_t* str, size_t len ) nssv_noexcept  // (2)
+    {
+      return nonstd::sv_lite::u16string_view{ str, len };
+    }
+
+    nssv_constexpr nonstd::sv_lite::u32string_view operator "" _sv( const char32_t* str, size_t len ) nssv_noexcept  // (3)
+    {
+      return nonstd::sv_lite::u32string_view{ str, len };
+    }
+
+    nssv_constexpr nonstd::sv_lite::wstring_view operator "" _sv( const wchar_t* str, size_t len ) nssv_noexcept  // (4)
+    {
+      return nonstd::sv_lite::wstring_view{ str, len };
+    }
+
+#endif // nssv_CONFIG_USR_SV_OPERATOR
+
+  }}} // namespace nonstd::literals::string_view_literals
+
+#endif
+
+//
+// Extensions for std::string:
+//
+
+#if nssv_CONFIG_CONVERSION_STD_STRING_FREE_FUNCTIONS
+
+namespace nonstd {
+namespace sv_lite {
+
+// Exclude MSVC 14 (19.00): it yields ambiguous to_string():
+
+#if nssv_CPP11_OR_GREATER && nssv_COMPILER_MSVC_VERSION != 140
+
+template< class CharT, class Traits, class Allocator = std::allocator<CharT> >
+std::basic_string<CharT, Traits, Allocator>
+to_string( basic_string_view<CharT, Traits> v, Allocator const & a = Allocator() )
+{
+  return std::basic_string<CharT,Traits, Allocator>( v.begin(), v.end(), a );
+}
+
+#else
+
+template< class CharT, class Traits >
+std::basic_string<CharT, Traits>
+to_string( basic_string_view<CharT, Traits> v )
+{
+  return std::basic_string<CharT, Traits>( v.begin(), v.end() );
+}
+
+template< class CharT, class Traits, class Allocator >
+std::basic_string<CharT, Traits, Allocator>
+to_string( basic_string_view<CharT, Traits> v, Allocator const & a )
+{
+  return std::basic_string<CharT, Traits, Allocator>( v.begin(), v.end(), a );
+}
+
+#endif // nssv_CPP11_OR_GREATER
+
+template< class CharT, class Traits, class Allocator >
+basic_string_view<CharT, Traits>
+to_string_view( std::basic_string<CharT, Traits, Allocator> const & s )
+{
+  return basic_string_view<CharT, Traits>( s.data(), s.size() );
+}
+
+}} // namespace nonstd::sv_lite
+
+#endif // nssv_CONFIG_CONVERSION_STD_STRING_FREE_FUNCTIONS
+
+//
+// make types and algorithms available in namespace nonstd:
+//
+
+namespace nonstd {
+
+using sv_lite::basic_string_view;
+using sv_lite::string_view;
+using sv_lite::wstring_view;
+
+#if nssv_HAVE_WCHAR16_T
+using sv_lite::u16string_view;
+#endif
+#if nssv_HAVE_WCHAR32_T
+using sv_lite::u32string_view;
+#endif
+
+// literal "sv"
+
+using sv_lite::operator==;
+using sv_lite::operator!=;
+using sv_lite::operator<;
+using sv_lite::operator<=;
+using sv_lite::operator>;
+using sv_lite::operator>=;
+
+using sv_lite::operator<<;
+
+#if nssv_CONFIG_CONVERSION_STD_STRING_FREE_FUNCTIONS
+using sv_lite::to_string;
+using sv_lite::to_string_view;
+#endif
+
+} // namespace nonstd
+
+// 24.4.5 Hash support (C++11):
+
+// Note: The hash value of a string view object is equal to the hash value of
+// the corresponding string object.
+
+#if nssv_HAVE_STD_HASH
+
+#include <functional>
+
+namespace std {
+
+template<>
+struct hash< nonstd::string_view >
+{
+public:
+  std::size_t operator()( nonstd::string_view v ) const nssv_noexcept
+  {
+    return std::hash<std::string>()( std::string( v.data(), v.size() ) );
+  }
+};
+
+template<>
+struct hash< nonstd::wstring_view >
+{
+public:
+  std::size_t operator()( nonstd::wstring_view v ) const nssv_noexcept
+  {
+    return std::hash<std::wstring>()( std::wstring( v.data(), v.size() ) );
+  }
+};
+
+template<>
+struct hash< nonstd::u16string_view >
+{
+public:
+  std::size_t operator()( nonstd::u16string_view v ) const nssv_noexcept
+  {
+    return std::hash<std::u16string>()( std::u16string( v.data(), v.size() ) );
+  }
+};
+
+template<>
+struct hash< nonstd::u32string_view >
+{
+public:
+  std::size_t operator()( nonstd::u32string_view v ) const nssv_noexcept
+  {
+    return std::hash<std::u32string>()( std::u32string( v.data(), v.size() ) );
+  }
+};
+
+} // namespace std
+
+#endif // nssv_HAVE_STD_HASH
+
+nssv_RESTORE_WARNINGS()
+
+#endif // nssv_HAVE_STD_STRING_VIEW
+#endif // NONSTD_SV_LITE_H_INCLUDED
diff --git a/include/behaviortree_cpp_v3/xml_parsing.h b/include/behaviortree_cpp_v3/xml_parsing.h
new file mode 100644 (file)
index 0000000..401dd1e
--- /dev/null
@@ -0,0 +1,44 @@
+#ifndef XML_PARSING_BT_H
+#define XML_PARSING_BT_H
+
+#include "behaviortree_cpp_v3/bt_parser.h"
+
+namespace BT
+{
+
+/**
+ * @brief The XMLParser is a class used to read the model
+ * of a BehaviorTree from file or text and instantiate the
+ * corresponding tree using the BehaviorTreeFactory.
+ */
+class XMLParser: public Parser
+{
+  public:
+    XMLParser(const BehaviorTreeFactory& factory);
+
+    ~XMLParser();
+
+    XMLParser(const XMLParser& other) = delete;
+    XMLParser& operator=(const XMLParser& other) = delete;
+
+    void loadFromFile(const std::string& filename) override;
+
+    void loadFromText(const std::string& xml_text) override;
+
+    Tree instantiateTree(const Blackboard::Ptr &root_blackboard) override;
+
+  private:
+
+    struct Pimpl;
+    Pimpl* _p;
+
+};
+
+void VerifyXML(const std::string& xml_text,
+               const std::set<std::string> &registered_nodes);
+
+std::string writeTreeNodesModelXML(const BehaviorTreeFactory& factory);
+
+}
+
+#endif   // XML_PARSING_BT_H
diff --git a/mkdocs.yml b/mkdocs.yml
new file mode 100644 (file)
index 0000000..a76d768
--- /dev/null
@@ -0,0 +1,56 @@
+site_name: BehaviorTree.CPP
+site_description: Introduction to Behavior Trees
+site_author: Davide Faconti
+
+copyright: 'Copyright &copy; 2018-2019 Davide Faconti, Eurecat'
+
+theme:
+  name: 'material'
+  language: en
+  logo: 'images/BT.png'
+  feature:
+    tabs: true
+  palette:
+    primary: indigo  
+    accent: Deep Purple
+  font:
+    text: Ubuntu
+    code: Roboto Mono
+  
+repo_name: 'BehaviorTree.CPP'
+repo_url: 'https://github.com/BehaviorTree/BehaviorTree.CPP'
+
+markdown_extensions:
+  - admonition
+  - pymdownx.details
+  - pymdownx.superfences
+  - codehilite:
+      guess_lang: true
+
+
+nav:
+    - Home: index.md
+    
+    - Learn the basics:
+       - Introduction to BT: BT_basics.md
+       - Getting started:    getting_started.md
+       - Sequence Nodes:     SequenceNode.md
+       - Fallback Nodes:     FallbackNode.md
+       - Decorators Nodes:   DecoratorNode.md
+       - The XML format:     xml_format.md
+       
+    - Tutorials: 
+
+       - "Summary":                           tutorials_summary.md
+       - "Tutorial 1: Create a Tree":         tutorial_01_first_tree.md
+       - "Tutorial 2: Basic Ports":           tutorial_02_basic_ports.md
+       - "Tutorial 3: Generic ports":         tutorial_03_generic_ports.md
+       - "Tutorial 4: Sequences":             tutorial_04_sequence_star.md
+       - "Tutorial 5: Subtrees and Loggers":  tutorial_05_subtrees.md
+       - "Tutorial 6: Ports remapping":       tutorial_06_subtree_ports.md
+       - "Tutorial 7: Wrap legacy code":      tutorial_07_legacy.md
+       - "Tutorial 8: Additional arguments":  tutorial_08_additional_args.md
+       - "Tutorial 9: Coroutines":            tutorial_09_coroutines.md
+       
+    - Migration Guide:
+       - Migrate from Version 2.X:  MigrationGuide.md        
diff --git a/package.xml b/package.xml
new file mode 100644 (file)
index 0000000..4bb9780
--- /dev/null
@@ -0,0 +1,32 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>behaviortree_cpp_v3</name>
+  <version>3.5.6</version>
+  <description>
+  This package provides the Behavior Trees core library.
+  </description>
+
+  <maintainer email="davide.faconti@gmail.com">Davide Faconti</maintainer>
+
+  <license>MIT</license>
+
+  <author>Michele Colledanchise</author>
+  <author>Davide Faconti</author>
+
+  <buildtool_depend condition="$ROS_VERSION == 1">catkin</buildtool_depend>
+  <depend condition="$ROS_VERSION == 1">roslib</depend>
+
+  <buildtool_depend condition="$ROS_VERSION == 2">ament_cmake</buildtool_depend>
+  <depend condition="$ROS_VERSION == 2">rclcpp</depend>
+
+  <depend>libzmq3-dev</depend>
+  <depend>libncurses-dev</depend>
+  
+  <test_depend condition="$ROS_VERSION == 2">ament_cmake_gtest</test_depend>
+
+  <export>
+      <build_type condition="$ROS_VERSION == 1">catkin</build_type>
+      <build_type condition="$ROS_VERSION == 2">ament_cmake</build_type>
+  </export>
+
+</package>
diff --git a/run_clang_format.sh b/run_clang_format.sh
new file mode 100755 (executable)
index 0000000..cf3d51a
--- /dev/null
@@ -0,0 +1,3 @@
+#!/bin/bash
+
+find . -name '*.h' -or -name '*.hpp' -or -name '*.cpp' | xargs clang-format-3.8 -i -style=file $1
diff --git a/sample_nodes/CMakeLists.txt b/sample_nodes/CMakeLists.txt
new file mode 100644 (file)
index 0000000..f156368
--- /dev/null
@@ -0,0 +1,39 @@
+cmake_minimum_required(VERSION 3.5.1)
+
+include_directories( ../include )
+
+# compile as static libraries
+
+set(CMAKE_DEBUG_POSTFIX "")
+
+add_library(bt_sample_nodes STATIC
+    crossdoor_nodes.cpp
+    dummy_nodes.cpp
+    movebase_node.cpp )
+
+target_link_libraries(bt_sample_nodes PRIVATE ${BEHAVIOR_TREE_LIBRARY})
+set_target_properties(bt_sample_nodes PROPERTIES ARCHIVE_OUTPUT_DIRECTORY
+    ${BEHAVIOR_TREE_LIB_DESTINATION} )
+
+# to create a plugin, compile them in this way instead
+
+add_library(crossdoor_nodes_dyn SHARED crossdoor_nodes.cpp )
+target_link_libraries(crossdoor_nodes_dyn PRIVATE ${BEHAVIOR_TREE_LIBRARY})
+target_compile_definitions(crossdoor_nodes_dyn PRIVATE  BT_PLUGIN_EXPORT )
+set_target_properties(crossdoor_nodes_dyn PROPERTIES LIBRARY_OUTPUT_DIRECTORY
+    ${BEHAVIOR_TREE_BIN_DESTINATION} )
+
+add_library(dummy_nodes_dyn     SHARED dummy_nodes.cpp )
+target_link_libraries(dummy_nodes_dyn PRIVATE ${BEHAVIOR_TREE_LIBRARY})
+target_compile_definitions(dummy_nodes_dyn  PRIVATE BT_PLUGIN_EXPORT)
+set_target_properties(dummy_nodes_dyn PROPERTIES LIBRARY_OUTPUT_DIRECTORY
+    ${BEHAVIOR_TREE_BIN_DESTINATION} )
+
+
+add_library(movebase_node_dyn   SHARED movebase_node.cpp )
+target_link_libraries(movebase_node_dyn PRIVATE ${BEHAVIOR_TREE_LIBRARY})
+target_compile_definitions(movebase_node_dyn PRIVATE  BT_PLUGIN_EXPORT )
+set_target_properties(movebase_node_dyn PROPERTIES LIBRARY_OUTPUT_DIRECTORY
+    ${BEHAVIOR_TREE_BIN_DESTINATION} )
+
+
diff --git a/sample_nodes/crossdoor_nodes.cpp b/sample_nodes/crossdoor_nodes.cpp
new file mode 100644 (file)
index 0000000..1b37c81
--- /dev/null
@@ -0,0 +1,80 @@
+#include "crossdoor_nodes.h"
+
+// This function must be implemented in the .cpp file to create
+// a plugin that can be loaded at run-time
+BT_REGISTER_NODES(factory)
+{
+    CrossDoor::RegisterNodes(factory);
+}
+
+// For simplicity, in this example the status of the door is not shared
+// using ports and blackboards
+static bool _door_open   = false;
+static bool _door_locked = true;
+
+NodeStatus CrossDoor::IsDoorOpen()
+{
+    SleepMS(500);
+    return _door_open ? NodeStatus::SUCCESS : NodeStatus::FAILURE;
+}
+
+NodeStatus CrossDoor::IsDoorLocked()
+{
+    SleepMS(500);
+    return _door_locked ? NodeStatus::SUCCESS : NodeStatus::FAILURE;
+}
+
+NodeStatus CrossDoor::UnlockDoor()
+{
+    if( _door_locked )
+    {
+        SleepMS(2000);
+        _door_locked = false;
+    }
+    return NodeStatus::SUCCESS;
+}
+
+NodeStatus CrossDoor::PassThroughDoor()
+{
+    SleepMS(1000);
+    return _door_open ? NodeStatus::SUCCESS : NodeStatus::FAILURE;
+}
+
+NodeStatus CrossDoor::PassThroughWindow()
+{
+    SleepMS(1000);
+    return NodeStatus::SUCCESS;
+}
+
+NodeStatus CrossDoor::OpenDoor()
+{
+    if (_door_locked)
+    {
+        return NodeStatus::FAILURE;
+    }
+    SleepMS(2000);
+    _door_open = true;
+    return NodeStatus::SUCCESS;
+}
+
+NodeStatus CrossDoor::CloseDoor()
+{
+    if (_door_open)
+    {
+        SleepMS(1500);
+        _door_open = false;
+    }
+    return NodeStatus::SUCCESS;
+}
+
+// Register at once all the Actions and Conditions in this file
+void CrossDoor::RegisterNodes(BehaviorTreeFactory& factory)
+{
+    factory.registerSimpleCondition("IsDoorOpen", std::bind(IsDoorOpen));
+    factory.registerSimpleAction("PassThroughDoor", std::bind(PassThroughDoor));
+    factory.registerSimpleAction("PassThroughWindow", std::bind(PassThroughWindow));
+    factory.registerSimpleAction("OpenDoor", std::bind(OpenDoor));
+    factory.registerSimpleAction("CloseDoor", std::bind(CloseDoor));
+    factory.registerSimpleCondition("IsDoorLocked", std::bind(IsDoorLocked));
+    factory.registerSimpleAction("UnlockDoor", std::bind(UnlockDoor));
+}
diff --git a/sample_nodes/crossdoor_nodes.h b/sample_nodes/crossdoor_nodes.h
new file mode 100644 (file)
index 0000000..876e5b2
--- /dev/null
@@ -0,0 +1,29 @@
+#pragma once
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+using namespace BT;
+
+namespace CrossDoor
+{
+inline void SleepMS(int ms)
+{
+    std::this_thread::sleep_for(std::chrono::milliseconds(ms));
+}
+
+BT::NodeStatus IsDoorOpen();
+
+BT::NodeStatus IsDoorLocked();
+
+BT::NodeStatus UnlockDoor();
+
+BT::NodeStatus PassThroughDoor();
+
+BT::NodeStatus PassThroughWindow();
+
+BT::NodeStatus OpenDoor();
+
+BT::NodeStatus CloseDoor();
+
+void RegisterNodes(BT::BehaviorTreeFactory& factory);
+}
diff --git a/sample_nodes/dummy_nodes.cpp b/sample_nodes/dummy_nodes.cpp
new file mode 100644 (file)
index 0000000..e3c8193
--- /dev/null
@@ -0,0 +1,63 @@
+#include "dummy_nodes.h"
+
+// This function must be implemented in the .cpp file to create
+// a plugin that can be loaded at run-time
+BT_REGISTER_NODES(factory)
+{
+    DummyNodes::RegisterNodes(factory);
+}
+
+namespace DummyNodes
+{
+
+BT::NodeStatus CheckBattery()
+{
+    std::cout << "[ Battery: OK ]" << std::endl;
+    return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus GripperInterface::open()
+{
+    _opened = true;
+    std::cout << "GripperInterface::open" << std::endl;
+    return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus GripperInterface::close()
+{
+    std::cout << "GripperInterface::close" << std::endl;
+    _opened = false;
+    return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus ApproachObject::tick()
+{
+    std::cout << "ApproachObject: " << this->name() << std::endl;
+    return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus SaySomething::tick()
+{
+    auto msg = getInput<std::string>("message");
+    if (!msg)
+    {
+        throw BT::RuntimeError( "missing required input [message]: ", msg.error() );
+    }
+
+    std::cout << "Robot says: " << msg.value() << std::endl;
+    return BT::NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus SaySomethingSimple(BT::TreeNode &self)
+{
+    auto msg = self.getInput<std::string>("message");
+    if (!msg)
+    {
+        throw BT::RuntimeError( "missing required input [message]: ", msg.error() );
+    }
+
+    std::cout << "Robot says: " << msg.value() << std::endl;
+    return BT::NodeStatus::SUCCESS;
+}
+
+}
diff --git a/sample_nodes/dummy_nodes.h b/sample_nodes/dummy_nodes.h
new file mode 100644 (file)
index 0000000..7a0d14e
--- /dev/null
@@ -0,0 +1,80 @@
+#ifndef SIMPLE_BT_NODES_H
+#define SIMPLE_BT_NODES_H
+
+#include "behaviortree_cpp_v3/behavior_tree.h"
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+namespace DummyNodes
+{
+
+BT::NodeStatus CheckBattery();
+
+class GripperInterface
+{
+  public:
+    GripperInterface() : _opened(true)
+    {
+    }
+
+    BT::NodeStatus open();
+
+    BT::NodeStatus close();
+
+  private:
+    bool _opened;
+};
+
+//--------------------------------------
+
+// Example of custom SyncActionNode (synchronous action)
+// without ports.
+class ApproachObject : public BT::SyncActionNode
+{
+  public:
+    ApproachObject(const std::string& name) :
+        BT::SyncActionNode(name, {})
+    {
+    }
+
+    // You must override the virtual function tick()
+    BT::NodeStatus tick() override;
+};
+
+// Example of custom SyncActionNode (synchronous action)
+// with an input port.
+class SaySomething : public BT::SyncActionNode
+{
+  public:
+    SaySomething(const std::string& name, const BT::NodeConfiguration& config)
+      : BT::SyncActionNode(name, config)
+    {
+    }
+
+    // You must override the virtual function tick()
+    BT::NodeStatus tick() override;
+
+    // It is mandatory to define this static method.
+    static BT::PortsList providedPorts()
+    {
+        return{ BT::InputPort<std::string>("message") };
+    }
+};
+
+//Same as class SaySomething, but to be registered with SimpleActionNode
+BT::NodeStatus SaySomethingSimple(BT::TreeNode& self);
+
+
+inline void RegisterNodes(BT::BehaviorTreeFactory& factory)
+{
+    static GripperInterface grip_singleton;
+
+    factory.registerSimpleCondition("CheckBattery", std::bind(CheckBattery));
+    factory.registerSimpleAction("OpenGripper", std::bind(&GripperInterface::open, &grip_singleton));
+    factory.registerSimpleAction("CloseGripper", std::bind(&GripperInterface::close, &grip_singleton));
+    factory.registerNodeType<ApproachObject>("ApproachObject");
+    factory.registerNodeType<SaySomething>("SaySomething");
+}
+
+} // end namespace
+
+#endif   // SIMPLE_BT_NODES_H
diff --git a/sample_nodes/movebase_node.cpp b/sample_nodes/movebase_node.cpp
new file mode 100644 (file)
index 0000000..9c84167
--- /dev/null
@@ -0,0 +1,39 @@
+#include "movebase_node.h"
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+// This function must be implemented in the .cpp file to create
+// a plugin that can be loaded at run-time
+BT_REGISTER_NODES(factory)
+{
+    factory.registerNodeType<MoveBaseAction>("MoveBase");
+}
+
+BT::NodeStatus MoveBaseAction::tick()
+{
+    Pose2D goal;
+    if ( !getInput<Pose2D>("goal", goal))
+    {
+        throw BT::RuntimeError("missing required input [goal]");
+    }
+
+    printf("[ MoveBase: STARTED ]. goal: x=%.f y=%.1f theta=%.2f\n", goal.x, goal.y, goal.theta);
+
+    _halt_requested.store(false);
+    int count = 0;
+
+    // Pretend that "computing" takes 250 milliseconds.
+    // It is up to you to check periodicall _halt_requested and interrupt
+    // this tick() if it is true.
+    while (!_halt_requested && count++ < 25)
+    {
+        SleepMS(10);
+    }
+
+    std::cout << "[ MoveBase: FINISHED ]" << std::endl;
+    return _halt_requested ? BT::NodeStatus::FAILURE : BT::NodeStatus::SUCCESS;
+}
+
+void MoveBaseAction::halt()
+{
+    _halt_requested.store(true);
+}
diff --git a/sample_nodes/movebase_node.h b/sample_nodes/movebase_node.h
new file mode 100644 (file)
index 0000000..4a7df1b
--- /dev/null
@@ -0,0 +1,71 @@
+#ifndef MOVEBASE_BT_NODES_H
+#define MOVEBASE_BT_NODES_H
+
+#include "behaviortree_cpp_v3/behavior_tree.h"
+
+// Custom type
+struct Pose2D
+{
+    double x, y, theta;
+};
+
+inline void SleepMS(int ms)
+{
+    std::this_thread::sleep_for(std::chrono::milliseconds(ms));
+}
+
+namespace BT
+{
+// This template specialization is needed only if you want
+// to AUTOMATICALLY convert a NodeParameter into a Pose2D
+// In other words, implement it if you want to be able to do:
+//
+//   TreeNode::getInput<Pose2D>(key, ...)
+//
+template <> inline
+Pose2D convertFromString(StringView key)
+{
+    // three real numbers separated by semicolons
+    auto parts = BT::splitString(key, ';');
+    if (parts.size() != 3)
+    {
+        throw BT::RuntimeError("invalid input)");
+    }
+    else
+    {
+        Pose2D output;
+        output.x     = convertFromString<double>(parts[0]);
+        output.y     = convertFromString<double>(parts[1]);
+        output.theta = convertFromString<double>(parts[2]);
+        return output;
+    }
+}
+} // end namespace BT
+
+// This is an asynchronous operation that will run in a separate thread.
+// It requires the input port "goal".
+
+class MoveBaseAction : public BT::AsyncActionNode
+{
+  public:
+    // Any TreeNode with ports must have a constructor with this signature
+    MoveBaseAction(const std::string& name, const BT::NodeConfiguration& config)
+      : AsyncActionNode(name, config)
+    {
+    }
+
+    // It is mandatory to define this static method.
+    static BT::PortsList providedPorts()
+    {
+        return{ BT::InputPort<Pose2D>("goal") };
+    }
+
+    BT::NodeStatus tick() override;
+
+    virtual void halt() override;
+
+  private:
+    std::atomic_bool _halt_requested;
+};
+
+#endif   // MOVEBASE_BT_NODES_H
diff --git a/src/action_node.cpp b/src/action_node.cpp
new file mode 100644 (file)
index 0000000..79fe35e
--- /dev/null
@@ -0,0 +1,207 @@
+/* Copyright (C) 2015-2018 Michele Colledanchise -  All Rights Reserved
+ * Copyright (C) 2018-2020 Davide Faconti, Eurecat -  All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#include "behaviortree_cpp_v3/action_node.h"
+
+using namespace BT;
+
+ActionNodeBase::ActionNodeBase(const std::string& name, const NodeConfiguration& config)
+  : LeafNode::LeafNode(name, config)
+{
+}
+
+
+//-------------------------------------------------------
+
+SimpleActionNode::SimpleActionNode(const std::string& name,
+                                   SimpleActionNode::TickFunctor tick_functor,
+                                   const NodeConfiguration& config)
+  : SyncActionNode(name, config), tick_functor_(std::move(tick_functor))
+{
+}
+
+NodeStatus SimpleActionNode::tick()
+{
+    NodeStatus prev_status = status();
+
+    if (prev_status == NodeStatus::IDLE)
+    {
+        setStatus(NodeStatus::RUNNING);
+        prev_status = NodeStatus::RUNNING;
+    }
+
+    NodeStatus status = tick_functor_(*this);
+    if (status != prev_status)
+    {
+        setStatus(status);
+    }
+    return status;
+}
+
+//-------------------------------------------------------
+
+SyncActionNode::SyncActionNode(const std::string &name, const NodeConfiguration& config):
+  ActionNodeBase(name, config)
+{}
+
+NodeStatus SyncActionNode::executeTick()
+{
+  auto stat = ActionNodeBase::executeTick();
+  if( stat == NodeStatus::RUNNING)
+  {
+    throw LogicError("SyncActionNode MUST never return RUNNING");
+  }
+  return stat;
+}
+
+
+//-------------------------------------
+#ifndef BT_NO_COROUTINES
+
+#ifdef BT_BOOST_COROUTINE2
+#include <boost/coroutine2/all.hpp>
+using namespace boost::coroutines2;
+#endif
+
+#ifdef BT_BOOST_COROUTINE
+#include <boost/coroutine/all.hpp>
+using namespace boost::coroutines;
+#endif
+
+struct CoroActionNode::Pimpl
+{
+    std::unique_ptr<coroutine<void>::pull_type> coro;
+    std::function<void(coroutine<void>::push_type & yield)> func;
+    coroutine<void>::push_type * yield_ptr;
+};
+
+CoroActionNode::CoroActionNode(const std::string &name,
+                               const NodeConfiguration& config):
+ ActionNodeBase (name, config), _p( new Pimpl)
+{
+    _p->func = [this](coroutine<void>::push_type & yield) {
+        _p->yield_ptr = &yield;
+        setStatus(tick());
+    };
+}
+
+CoroActionNode::~CoroActionNode()
+{
+}
+
+void CoroActionNode::setStatusRunningAndYield()
+{
+    setStatus( NodeStatus::RUNNING );
+    (*_p->yield_ptr)();
+}
+
+NodeStatus CoroActionNode::executeTick()
+{
+    if( !(_p->coro) || !(*_p->coro) )
+    {
+        _p->coro.reset( new coroutine<void>::pull_type(_p->func) );
+        return status();
+    }
+
+    if( status() == NodeStatus::RUNNING && (bool)_p->coro )
+    {
+        (*_p->coro)();
+    }
+
+    return status();
+}
+
+void CoroActionNode::halt()
+{
+    _p->coro.reset();
+}
+#endif
+
+
+
+NodeStatus StatefulActionNode::tick()
+{
+  const NodeStatus initial_status = status();
+
+  if( initial_status == NodeStatus::IDLE )
+  {
+    NodeStatus new_status = onStart();
+    if( new_status == NodeStatus::IDLE)
+    {
+      throw std::logic_error("AsyncActionNode2::onStart() must not return IDLE");
+    }
+    return new_status;
+  }
+  //------------------------------------------
+  if( initial_status == NodeStatus::RUNNING )
+  {
+    NodeStatus new_status = onRunning();
+    if( new_status == NodeStatus::IDLE)
+    {
+      throw std::logic_error("AsyncActionNode2::onRunning() must not return IDLE");
+    }
+    return new_status;
+  }
+  //------------------------------------------
+  return initial_status;
+}
+
+void StatefulActionNode::halt()
+{
+  if( status() == NodeStatus::RUNNING)
+  {
+    onHalted();
+  }
+  setStatus(NodeStatus::IDLE);
+}
+
+NodeStatus BT::AsyncActionNode::executeTick()
+{
+    //send signal to other thread.
+    // The other thread is in charge for changing the status
+    if (status() == NodeStatus::IDLE)
+    {
+        setStatus( NodeStatus::RUNNING );
+        halt_requested_ = false;
+        thread_handle_ = std::async(std::launch::async, [this]() {
+
+            try {
+                setStatus(tick());
+            }
+            catch (std::exception&)
+            {
+                std::cerr << "\nUncaught exception from the method tick(): ["
+                          << registrationName() << "/" << name() << "]\n" << std::endl;
+                exptr_ = std::current_exception();
+                thread_handle_.wait();
+            }
+            return status();
+        });
+    }
+
+    if( exptr_ )
+    {
+        std::rethrow_exception(exptr_);
+    }
+    return status();
+}
+
+void AsyncActionNode::halt()
+{
+    halt_requested_.store(true);
+
+    if( thread_handle_.valid() ){
+        thread_handle_.wait();
+    }
+    thread_handle_ = {};
+}
diff --git a/src/basic_types.cpp b/src/basic_types.cpp
new file mode 100644 (file)
index 0000000..894ad48
--- /dev/null
@@ -0,0 +1,328 @@
+#include "behaviortree_cpp_v3/basic_types.h"
+#include <cstdlib>
+#include <cstring>
+#include <clocale>
+
+namespace BT
+{
+
+template <>
+std::string toStr<NodeStatus>(NodeStatus status)
+{
+    switch (status)
+    {
+        case NodeStatus::SUCCESS:
+            return "SUCCESS";
+        case NodeStatus::FAILURE:
+            return "FAILURE";
+        case NodeStatus::RUNNING:
+            return "RUNNING";
+        case NodeStatus::IDLE:
+            return "IDLE";
+    }
+    return "";
+}
+
+std::string toStr(std::string value)
+{
+  return value;
+}
+
+std::string toStr(NodeStatus status, bool colored)
+{
+    if (!colored)
+    {
+        return toStr(status);
+    }
+    else
+    {
+        switch (status)
+        {
+            case NodeStatus::SUCCESS:
+                return "\x1b[32m"
+                        "SUCCESS"
+                        "\x1b[0m";   // RED
+            case NodeStatus::FAILURE:
+                return "\x1b[31m"
+                        "FAILURE"
+                        "\x1b[0m";   // GREEN
+            case NodeStatus::RUNNING:
+                return "\x1b[33m"
+                        "RUNNING"
+                        "\x1b[0m";   // YELLOW
+            case NodeStatus::IDLE:
+                return "\x1b[36m"
+                        "IDLE"
+                        "\x1b[0m";   // CYAN
+        }
+    }
+    return "Undefined";
+}
+
+
+
+template <>
+std::string toStr<PortDirection>(PortDirection direction)
+{
+    switch(direction)
+    {
+        case PortDirection::INPUT:  return "Input";
+        case PortDirection::OUTPUT: return "Output";
+        case PortDirection::INOUT:  return "InOut";
+    }
+    return "InOut";
+}
+
+
+template<> std::string toStr<NodeType>(NodeType type)
+{
+    switch (type)
+    {
+        case NodeType::ACTION:
+            return "Action";
+        case NodeType::CONDITION:
+            return "Condition";
+        case NodeType::DECORATOR:
+            return "Decorator";
+        case NodeType::CONTROL:
+            return "Control";
+        case NodeType::SUBTREE:
+            return "SubTree";
+        default:
+            return "Undefined";
+    }
+}
+
+
+template <>
+std::string convertFromString<std::string>(StringView str)
+{
+    return std::string( str.data(), str.size() );
+}
+
+
+template <>
+int convertFromString<int>(StringView str)
+{
+    return  std::stoi(str.data());
+}
+
+template <>
+long convertFromString<long>(StringView str)
+{
+  return  std::stol(str.data());
+}
+
+template <>
+unsigned convertFromString<unsigned>(StringView str)
+{
+    return unsigned(std::stoul(str.data()));
+}
+
+template <>
+unsigned long convertFromString<unsigned long>(StringView str)
+{
+    return std::stoul(str.data());
+}
+
+template <>
+double convertFromString<double>(StringView str)
+{
+    // see issue #120
+    // http://quick-bench.com/DWaXRWnxtxvwIMvZy2DxVPEKJnE
+
+    std::string old_locale = setlocale(LC_NUMERIC,nullptr);
+    setlocale(LC_NUMERIC,"C");
+    double val = std::stod(str.data());
+    setlocale(LC_NUMERIC, old_locale.c_str());
+    return val;
+}
+
+template <>
+float convertFromString<float>(StringView str)
+{
+    std::string old_locale = setlocale(LC_NUMERIC,nullptr);
+    setlocale(LC_NUMERIC,"C");
+    float val = std::stof(str.data());
+    setlocale(LC_NUMERIC, old_locale.c_str());
+    return val;
+}
+
+template <>
+std::vector<int> convertFromString<std::vector<int>>(StringView str)
+{
+    auto parts = splitString(str, ';');
+    std::vector<int> output;
+    output.reserve( parts.size() );
+    for(const StringView& part: parts)
+    {
+        char* end;
+        output.push_back( std::strtol( part.data(), &end, 10 ) );
+    }
+    return output;
+}
+
+template <>
+std::vector<double> convertFromString<std::vector<double>>(StringView str)
+{
+    auto parts = splitString(str, ';');
+    std::vector<double> output;
+    output.reserve( parts.size() );
+    for(const StringView& part: parts)
+    {
+        char* end;
+        output.push_back( std::strtod( part.data(), &end ) );
+    }
+    return output;
+}
+
+template <>
+bool convertFromString<bool>(StringView str)
+{
+    if (str.size() == 1)
+    {
+        if (str[0] == '0')
+        {
+            return false;
+        }
+        if (str[0] == '1')
+        {
+            return true;
+        }
+    }
+    else if (str.size() == 4)
+    {
+        if (str == "true" || str == "TRUE" || str == "True")
+        {
+            return true;
+        }
+    }
+    else if (str.size() == 5)
+    {
+        if (str == "false" || str == "FALSE" || str == "False")
+        {
+            return false;
+        }
+    }
+    throw RuntimeError("convertFromString(): invalid bool conversion");
+}
+
+template <>
+NodeStatus convertFromString<NodeStatus>(StringView str)
+{
+    if( str == "IDLE" )    return NodeStatus::IDLE;
+    if( str == "RUNNING" ) return NodeStatus::RUNNING;
+    if( str == "SUCCESS" ) return NodeStatus::SUCCESS;
+    if( str == "FAILURE" ) return NodeStatus::FAILURE;
+    throw RuntimeError(std::string("Cannot convert this to NodeStatus: ") + static_cast<std::string>(str) );
+}
+
+template <>
+NodeType convertFromString<NodeType>(StringView str)
+{
+    if( str == "Action" )    return NodeType::ACTION;
+    if( str == "Condition" ) return NodeType::CONDITION;
+    if( str == "Control" )   return NodeType::CONTROL;
+    if( str == "Decorator" ) return NodeType::DECORATOR;
+    if( str == "SubTree" || str == "SubTreePlus" ) return NodeType::SUBTREE;
+    return NodeType::UNDEFINED;
+}
+
+template <>
+PortDirection convertFromString<PortDirection>(StringView str)
+{
+    if( str == "Input"  || str == "INPUT" )    return PortDirection::INPUT;
+    if( str == "Output" || str == "OUTPUT")    return PortDirection::OUTPUT;
+    return PortDirection::INOUT;
+}
+
+
+std::ostream& operator<<(std::ostream& os, const NodeType& type)
+{
+    os << toStr(type);
+    return os;
+}
+
+std::ostream& operator<<(std::ostream& os, const NodeStatus& status)
+{
+    os << toStr(status);
+    return os;
+}
+
+std::ostream& operator<<(std::ostream& os, const PortDirection& type)
+{
+    os << toStr(type);
+    return os;
+}
+
+std::vector<StringView> splitString(const StringView &strToSplit, char delimeter)
+{
+    std::vector<StringView> splitted_strings;
+    splitted_strings.reserve(4);
+
+    size_t pos = 0;
+    while( pos < strToSplit.size())
+    {
+        size_t new_pos = strToSplit.find_first_of(delimeter, pos);
+        if( new_pos == std::string::npos)
+        {
+           new_pos = strToSplit.size();
+        }
+        StringView sv = { &strToSplit.data()[pos], new_pos - pos };
+        splitted_strings.push_back( sv );
+        pos = new_pos + 1;
+    }
+    return splitted_strings;
+}
+
+PortDirection PortInfo::direction() const
+{
+    return _type;
+}
+
+const std::type_info* PortInfo::type() const
+{
+    return _info;
+}
+
+Any PortInfo::parseString(const char *str) const
+{
+    if( _converter)
+    {
+        return _converter(str);
+    }
+    return {};
+}
+
+Any PortInfo::parseString(const std::string &str) const
+{
+    if( _converter)
+    {
+        return _converter(str);
+    }
+    return {};
+}
+
+void PortInfo::setDescription(StringView description)
+{
+    description_ = static_cast<std::string>(description);
+}
+
+void PortInfo::setDefaultValue(StringView default_value_as_string)
+{
+    default_value_ = static_cast<std::string>(default_value_as_string);
+}
+
+const std::string &PortInfo::description() const
+{
+    return  description_;
+}
+
+const std::string &PortInfo::defaultValue() const
+{
+    return  default_value_;
+}
+
+
+
+}   // end namespace
diff --git a/src/behavior_tree.cpp b/src/behavior_tree.cpp
new file mode 100644 (file)
index 0000000..f50b23e
--- /dev/null
@@ -0,0 +1,110 @@
+/*  Copyright (C) 2018-2020 Davide Faconti, Eurecat -  All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#include "behaviortree_cpp_v3/behavior_tree.h"
+#include <cstring>
+
+namespace BT
+{
+void applyRecursiveVisitor(const TreeNode* node,
+                           const std::function<void(const TreeNode*)>& visitor)
+{
+    if (!node)
+    {
+        throw LogicError("One of the children of a DecoratorNode or ControlNode is nullptr");
+    }
+
+    visitor(node);
+
+    if (auto control = dynamic_cast<const BT::ControlNode*>(node))
+    {
+        for (const auto& child : control->children())
+        {
+            applyRecursiveVisitor(static_cast<const TreeNode*>(child), visitor);
+        }
+    }
+    else if (auto decorator = dynamic_cast<const BT::DecoratorNode*>(node))
+    {
+        applyRecursiveVisitor(decorator->child(), visitor);
+    }
+}
+
+void applyRecursiveVisitor(TreeNode* node, const std::function<void(TreeNode*)>& visitor)
+{
+    if (!node)
+    {
+        throw LogicError("One of the children of a DecoratorNode or ControlNode is nullptr");
+    }
+
+    visitor(node);
+
+    if (auto control = dynamic_cast<BT::ControlNode*>(node))
+    {
+        for (const auto& child : control->children())
+        {
+            applyRecursiveVisitor(child, visitor);
+        }
+    }
+    else if (auto decorator = dynamic_cast<BT::DecoratorNode*>(node))
+    {
+        applyRecursiveVisitor(decorator->child(), visitor);
+    }
+}
+
+void printTreeRecursively(const TreeNode* root_node)
+{
+    std::function<void(unsigned, const BT::TreeNode*)> recursivePrint;
+
+    recursivePrint = [&recursivePrint](unsigned indent, const BT::TreeNode* node) {
+        for (unsigned i = 0; i < indent; i++)
+        {
+            std::cout << "   ";
+        }
+        if (!node)
+        {
+            std::cout << "!nullptr!" << std::endl;
+            return;
+        }
+        std::cout << node->name() << std::endl;
+        indent++;
+
+        if (auto control = dynamic_cast<const BT::ControlNode*>(node))
+        {
+            for (const auto& child : control->children())
+            {
+                recursivePrint(indent, child);
+            }
+        }
+        else if (auto decorator = dynamic_cast<const BT::DecoratorNode*>(node))
+        {
+            recursivePrint(indent, decorator->child());
+        }
+    };
+
+    std::cout << "----------------" << std::endl;
+    recursivePrint(0, root_node);
+    std::cout << "----------------" << std::endl;
+}
+
+void buildSerializedStatusSnapshot(TreeNode* root_node, SerializedTreeStatus& serialized_buffer)
+{
+    serialized_buffer.clear();
+
+    auto visitor = [&serialized_buffer](const TreeNode* node) {
+        serialized_buffer.push_back(
+            std::make_pair(node->UID(), static_cast<uint8_t>(node->status())));
+    };
+
+    applyRecursiveVisitor(root_node, visitor);
+}
+
+} // end namespace
diff --git a/src/blackboard.cpp b/src/blackboard.cpp
new file mode 100644 (file)
index 0000000..f86fddd
--- /dev/null
@@ -0,0 +1,89 @@
+#include "behaviortree_cpp_v3/blackboard.h"
+
+namespace BT{
+
+void Blackboard::setPortInfo(std::string key, const PortInfo& info)
+{
+    std::unique_lock<std::mutex> lock(mutex_);
+
+    if( auto parent = parent_bb_.lock())
+    {
+        auto remapping_it = internal_to_external_.find(key);
+        if( remapping_it != internal_to_external_.end())
+        {
+            parent->setPortInfo( remapping_it->second, info );
+        }
+    }
+
+    auto it = storage_.find(key);
+    if( it == storage_.end() )
+    {
+        storage_.insert( { std::move(key), Entry(info) } );
+    }
+    else{
+        auto old_type = it->second.port_info.type();
+        if( old_type && old_type != info.type() )
+        {
+            throw LogicError( "Blackboard::set() failed: once declared, the type of a port shall not change. "
+                             "Declared type [",     BT::demangle( old_type ),
+                             "] != current type [", BT::demangle( info.type() ), "]" );
+        }
+    }
+}
+
+const PortInfo* Blackboard::portInfo(const std::string &key)
+{
+    std::unique_lock<std::mutex> lock(mutex_);
+    auto it = storage_.find(key);
+    if( it == storage_.end() )
+    {
+        return nullptr;
+    }
+    return &(it->second.port_info);
+}
+
+void Blackboard::addSubtreeRemapping(StringView internal, StringView external)
+{
+    internal_to_external_.insert( {static_cast<std::string>(internal), static_cast<std::string>(external)} );
+}
+
+void Blackboard::debugMessage() const
+{
+    for(const auto& entry_it: storage_)
+    {
+        auto port_type = entry_it.second.port_info.type();
+        if( !port_type )
+        {
+            port_type = &( entry_it.second.value.type() );
+        }
+
+        std::cout <<  entry_it.first << " (" << demangle( port_type ) << ") -> ";
+
+        if( auto parent = parent_bb_.lock())
+        {
+            auto remapping_it = internal_to_external_.find( entry_it.first );
+            if( remapping_it != internal_to_external_.end())
+            {
+                std::cout << "remapped to parent [" << remapping_it->second << "]" <<std::endl;
+                continue;
+            }
+        }
+        std::cout << ((entry_it.second.value.empty()) ? "empty" : "full") <<  std::endl;
+    }
+}
+
+std::vector<StringView> Blackboard::getKeys() const
+{
+    if( storage_.empty() ){
+        return {};    
+    }
+    std::vector<StringView> out;
+    out.reserve( storage_.size() );
+    for(const auto& entry_it: storage_)
+    {
+        out.push_back( entry_it.first );
+    }
+    return out;
+}
+
+}
diff --git a/src/bt_factory.cpp b/src/bt_factory.cpp
new file mode 100644 (file)
index 0000000..b850129
--- /dev/null
@@ -0,0 +1,284 @@
+/*  Copyright (C) 2018-2020 Davide Faconti, Eurecat -  All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+#include "behaviortree_cpp_v3/utils/shared_library.h"
+#include "behaviortree_cpp_v3/xml_parsing.h"
+
+#ifdef USING_ROS
+#include "filesystem/path.h"
+#include <ros/package.h>
+#endif
+
+namespace BT
+{
+BehaviorTreeFactory::BehaviorTreeFactory()
+{
+    registerNodeType<FallbackNode>("Fallback");
+    registerNodeType<SequenceNode>("Sequence");
+    registerNodeType<SequenceStarNode>("SequenceStar");
+    registerNodeType<ParallelNode>("Parallel");
+    registerNodeType<ReactiveSequence>("ReactiveSequence");
+    registerNodeType<ReactiveFallback>("ReactiveFallback");
+    registerNodeType<IfThenElseNode>("IfThenElse");
+    registerNodeType<WhileDoElseNode>("WhileDoElse");
+
+    registerNodeType<InverterNode>("Inverter");
+    registerNodeType<RetryNode>("RetryUntilSuccesful");
+    registerNodeType<KeepRunningUntilFailureNode>("KeepRunningUntilFailure");
+    registerNodeType<RepeatNode>("Repeat");
+    registerNodeType<TimeoutNode<>>("Timeout");
+    registerNodeType<DelayNode>("Delay");
+
+    registerNodeType<ForceSuccessNode>("ForceSuccess");
+    registerNodeType<ForceFailureNode>("ForceFailure");
+
+    registerNodeType<AlwaysSuccessNode>("AlwaysSuccess");
+    registerNodeType<AlwaysFailureNode>("AlwaysFailure");
+    registerNodeType<SetBlackboard>("SetBlackboard");
+
+    registerNodeType<SubtreeNode>("SubTree");
+    registerNodeType<SubtreePlusNode>("SubTreePlus");
+
+    registerNodeType<BlackboardPreconditionNode<int>>("BlackboardCheckInt");
+    registerNodeType<BlackboardPreconditionNode<double>>("BlackboardCheckDouble");
+    registerNodeType<BlackboardPreconditionNode<std::string>>("BlackboardCheckString");
+
+    registerNodeType<SwitchNode<2>>("Switch2");
+    registerNodeType<SwitchNode<3>>("Switch3");
+    registerNodeType<SwitchNode<4>>("Switch4");
+    registerNodeType<SwitchNode<5>>("Switch5");
+    registerNodeType<SwitchNode<6>>("Switch6");
+
+#ifdef NCURSES_FOUND
+    registerNodeType<ManualSelectorNode>("ManualSelector");
+#endif
+    for( const auto& it: builders_)
+    {
+        builtin_IDs_.insert( it.first );
+    }
+}
+
+bool BehaviorTreeFactory::unregisterBuilder(const std::string& ID)
+{
+    if( builtinNodes().count(ID) )
+    {
+        throw LogicError("You can not remove the builtin registration ID [", ID, "]");
+    }
+    auto it = builders_.find(ID);
+    if (it == builders_.end())
+    {
+        return false;
+    }
+    builders_.erase(ID);
+    manifests_.erase(ID);
+    return true;
+}
+
+void BehaviorTreeFactory::registerBuilder(const TreeNodeManifest& manifest, const NodeBuilder& builder)
+{
+    auto it = builders_.find( manifest.registration_ID);
+    if (it != builders_.end())
+    {
+        throw BehaviorTreeException("ID [", manifest.registration_ID, "] already registered");
+    }
+
+    builders_.insert(  {manifest.registration_ID, builder} );
+    manifests_.insert( {manifest.registration_ID, manifest} );
+}
+
+void BehaviorTreeFactory::registerSimpleCondition(const std::string& ID,
+                                                  const SimpleConditionNode::TickFunctor& tick_functor,
+                                                  PortsList ports)
+{
+    NodeBuilder builder = [tick_functor, ID](const std::string& name, const NodeConfiguration& config) {
+        return std::make_unique<SimpleConditionNode>(name, tick_functor, config);
+    };
+
+    TreeNodeManifest manifest = { NodeType::CONDITION, ID, std::move(ports) };
+    registerBuilder(manifest, builder);
+}
+
+void BehaviorTreeFactory::registerSimpleAction(const std::string& ID,
+                                               const SimpleActionNode::TickFunctor& tick_functor,
+                                               PortsList ports)
+{
+    NodeBuilder builder = [tick_functor, ID](const std::string& name, const NodeConfiguration& config) {
+        return std::make_unique<SimpleActionNode>(name, tick_functor, config);
+    };
+
+    TreeNodeManifest manifest = { NodeType::ACTION, ID, std::move(ports) };
+    registerBuilder(manifest, builder);
+}
+
+void BehaviorTreeFactory::registerSimpleDecorator(const std::string& ID,
+                                                  const SimpleDecoratorNode::TickFunctor& tick_functor,
+                                                  PortsList ports)
+{
+    NodeBuilder builder = [tick_functor, ID](const std::string& name, const NodeConfiguration& config) {
+        return std::make_unique<SimpleDecoratorNode>(name, tick_functor, config);
+    };
+
+    TreeNodeManifest manifest = { NodeType::DECORATOR, ID, std::move(ports) };
+    registerBuilder(manifest, builder);
+}
+
+void BehaviorTreeFactory::registerFromPlugin(const std::string& file_path)
+{
+    BT::SharedLibrary loader;
+    loader.load(file_path);
+    typedef void (*Func)(BehaviorTreeFactory&);
+
+    if (loader.hasSymbol(PLUGIN_SYMBOL))
+    {
+        Func func = (Func)loader.getSymbol(PLUGIN_SYMBOL);
+        func(*this);
+    }
+    else
+    {
+        std::cout << "ERROR loading library [" << file_path << "]: can't find symbol ["
+                  << PLUGIN_SYMBOL << "]" << std::endl;
+    }
+}
+
+#ifdef USING_ROS
+
+    #ifdef _WIN32
+const char os_pathsep(';');   // NOLINT
+#else
+const char os_pathsep(':');   // NOLINT
+#endif
+
+// This function is a copy from the one in class_loader_imp.hpp in ROS pluginlib
+// package, licensed under BSD.
+// https://github.com/ros/pluginlib
+std::vector<std::string> getCatkinLibraryPaths()
+{
+    std::vector<std::string> lib_paths;
+    const char* env = std::getenv("CMAKE_PREFIX_PATH");
+    if (env)
+    {
+        const std::string env_catkin_prefix_paths(env);
+        std::vector<BT::StringView> catkin_prefix_paths =
+            splitString(env_catkin_prefix_paths, os_pathsep);
+        for (BT::StringView catkin_prefix_path : catkin_prefix_paths)
+        {
+            filesystem::path path(static_cast<std::string>(catkin_prefix_path));
+            filesystem::path lib("lib");
+            lib_paths.push_back((path / lib).str());
+        }
+    }
+    return lib_paths;
+}
+
+void BehaviorTreeFactory::registerFromROSPlugins()
+{
+    std::vector<std::string> plugins;
+    ros::package::getPlugins("behaviortree_cpp_v3", "bt_lib_plugin", plugins, true);
+    std::vector<std::string> catkin_lib_paths = getCatkinLibraryPaths();
+
+    for (const auto& plugin : plugins)
+    {
+        auto filename = filesystem::path(plugin + BT::SharedLibrary::suffix());
+        for (const auto& lib_path : catkin_lib_paths)
+        {
+            const auto full_path = filesystem::path(lib_path) / filename;
+            if (full_path.exists())
+            {
+                std::cout << "Registering ROS plugins from " << full_path.str() << std::endl;
+                registerFromPlugin(full_path.str());
+                break;
+            }
+        }
+    }
+}
+#else
+
+    void BehaviorTreeFactory::registerFromROSPlugins()
+    {
+        throw RuntimeError("Using attribute [ros_pkg] in <include>, but this library was compiled "
+                           "without ROS support. Recompile the BehaviorTree.CPP using catkin");
+    }
+#endif
+
+std::unique_ptr<TreeNode> BehaviorTreeFactory::instantiateTreeNode(
+        const std::string& name,
+        const std::string& ID,
+        const NodeConfiguration& config) const
+{
+    auto it = builders_.find(ID);
+    if (it == builders_.end())
+    {
+        std::cerr << ID << " not included in this list:" << std::endl;
+        for (const auto& builder_it: builders_)
+        {
+            std::cerr << builder_it.first << std::endl;
+        }
+        throw RuntimeError("BehaviorTreeFactory: ID [", ID, "] not registered");
+    }
+
+    std::unique_ptr<TreeNode> node = it->second(name, config);
+    node->setRegistrationID( ID );
+    return node;
+}
+
+const std::unordered_map<std::string, NodeBuilder> &BehaviorTreeFactory::builders() const
+{
+    return builders_;
+}
+
+const std::unordered_map<std::string,TreeNodeManifest>& BehaviorTreeFactory::manifests() const
+{
+    return manifests_;
+}
+
+const std::set<std::string> &BehaviorTreeFactory::builtinNodes() const
+{
+    return builtin_IDs_;
+}
+
+Tree BehaviorTreeFactory::createTreeFromText(const std::string &text,
+                                             Blackboard::Ptr blackboard)
+{
+    XMLParser parser(*this);
+    parser.loadFromText(text);
+    auto tree = parser.instantiateTree(blackboard);
+    tree.manifests = this->manifests();
+    return tree;
+}
+
+Tree BehaviorTreeFactory::createTreeFromFile(const std::string &file_path,
+                                             Blackboard::Ptr blackboard)
+{
+    XMLParser parser(*this);
+    parser.loadFromFile(file_path);
+    auto tree = parser.instantiateTree(blackboard);
+    tree.manifests = this->manifests();
+    return tree;
+}
+
+Tree::~Tree()
+{
+    haltTree();
+}
+
+Blackboard::Ptr Tree::rootBlackboard()
+{
+    if( blackboard_stack.size() > 0)
+    {
+        return blackboard_stack.front();
+    }
+    return {};
+}
+
+
+}   // end namespace
diff --git a/src/condition_node.cpp b/src/condition_node.cpp
new file mode 100644 (file)
index 0000000..bd113e3
--- /dev/null
@@ -0,0 +1,33 @@
+/* Copyright (C) 2015-2018 Michele Colledanchise -  All Rights Reserved
+ * Copyright (C) 2018-2020 Davide Faconti, Eurecat -  All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#include "behaviortree_cpp_v3/condition_node.h"
+
+namespace BT
+{
+ConditionNode::ConditionNode(const std::string& name, const NodeConfiguration& config)
+  : LeafNode::LeafNode(name, config)
+{
+}
+
+SimpleConditionNode::SimpleConditionNode(const std::string& name, TickFunctor tick_functor,
+                                         const NodeConfiguration& config)
+  : ConditionNode(name, config), tick_functor_(std::move(tick_functor))
+{
+}
+
+NodeStatus SimpleConditionNode::tick()
+{
+    return tick_functor_(*this);
+}
+}
diff --git a/src/control_node.cpp b/src/control_node.cpp
new file mode 100644 (file)
index 0000000..ad9e14b
--- /dev/null
@@ -0,0 +1,70 @@
+/* Copyright (C) 2015-2018 Michele Colledanchise -  All Rights Reserved
+ * Copyright (C) 2018-2020 Davide Faconti, Eurecat -  All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#include "behaviortree_cpp_v3/control_node.h"
+
+namespace BT
+{
+ControlNode::ControlNode(const std::string& name, const NodeConfiguration& config)
+  : TreeNode::TreeNode(name, config)
+{
+}
+
+void ControlNode::addChild(TreeNode* child)
+{
+    children_nodes_.push_back(child);
+}
+
+size_t ControlNode::childrenCount() const
+{
+    return children_nodes_.size();
+}
+
+void ControlNode::halt()
+{
+    haltChildren();
+    setStatus(NodeStatus::IDLE);
+}
+
+const std::vector<TreeNode*>& ControlNode::children() const
+{
+    return children_nodes_;
+}
+
+void ControlNode::haltChild(size_t i)
+{
+    auto child = children_nodes_[i];
+    if (child->status() == NodeStatus::RUNNING)
+    {
+        child->halt();
+    }
+    child->setStatus(NodeStatus::IDLE);
+}
+
+void ControlNode::haltChildren()
+{
+    for (size_t i = 0; i < children_nodes_.size(); i++)
+    {
+        haltChild(i);
+    }
+}
+
+void ControlNode::haltChildren(size_t first)
+{
+    for (size_t i = first; i < children_nodes_.size(); i++)
+    {
+        haltChild(i);
+    }
+}
+
+} // end namespace
diff --git a/src/controls/fallback_node.cpp b/src/controls/fallback_node.cpp
new file mode 100644 (file)
index 0000000..59ab331
--- /dev/null
@@ -0,0 +1,78 @@
+/* Copyright (C) 2015-2018 Michele Colledanchise -  All Rights Reserved
+ * Copyright (C) 2018-2020 Davide Faconti, Eurecat -  All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#include "behaviortree_cpp_v3/controls/fallback_node.h"
+#include "behaviortree_cpp_v3/action_node.h"
+namespace BT
+{
+
+FallbackNode::FallbackNode(const std::string& name)
+    : ControlNode::ControlNode(name, {} )
+    ,current_child_idx_(0)
+{
+    setRegistrationID("Fallback");
+}
+
+NodeStatus FallbackNode::tick()
+{
+    const size_t children_count = children_nodes_.size();
+
+    setStatus(NodeStatus::RUNNING);
+
+    while (current_child_idx_ < children_count)
+    {
+        TreeNode* current_child_node = children_nodes_[current_child_idx_];
+        const NodeStatus child_status = current_child_node->executeTick();
+
+        switch (child_status)
+        {
+            case NodeStatus::RUNNING:
+            {
+                return child_status;
+            }
+            case NodeStatus::SUCCESS:
+            {
+                haltChildren();
+                current_child_idx_ = 0;
+                return child_status;
+            }
+            case NodeStatus::FAILURE:
+            {
+                current_child_idx_++;
+            }
+            break;
+
+            case NodeStatus::IDLE:
+            {
+                throw LogicError("A child node must never return IDLE");
+            }
+        }   // end switch
+    }       // end while loop
+
+    // The entire while loop completed. This means that all the children returned FAILURE.
+    if (current_child_idx_ == children_count)
+    {
+        haltChildren();
+        current_child_idx_ = 0;
+    }
+
+    return NodeStatus::FAILURE;
+}
+
+void FallbackNode::halt()
+{
+    current_child_idx_ = 0;
+    ControlNode::halt();
+}
+
+}
diff --git a/src/controls/if_then_else_node.cpp b/src/controls/if_then_else_node.cpp
new file mode 100644 (file)
index 0000000..bdba763
--- /dev/null
@@ -0,0 +1,83 @@
+/* Copyright (C) 2020 Davide Faconti -  All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#include "behaviortree_cpp_v3/controls/if_then_else_node.h"
+
+
+namespace BT
+{
+
+IfThenElseNode::IfThenElseNode(const std::string &name)
+  : ControlNode::ControlNode(name, {} )
+  , child_idx_(0)
+{
+  setRegistrationID("IfThenElse");
+}
+
+void IfThenElseNode::halt()
+{
+  child_idx_ = 0;
+  ControlNode::halt();
+}
+
+NodeStatus IfThenElseNode::tick()
+{
+  const size_t children_count = children_nodes_.size();
+
+  if(children_count != 2 && children_count != 3)
+  {
+    throw std::logic_error("IfThenElseNode must have either 2 or 3 children");
+  }
+
+  setStatus(NodeStatus::RUNNING);
+
+  if (child_idx_ == 0)
+  {
+    NodeStatus condition_status = children_nodes_[0]->executeTick();
+
+    if (condition_status == NodeStatus::RUNNING)
+    {
+      return condition_status;
+    }
+    else if (condition_status == NodeStatus::SUCCESS)
+    {
+      child_idx_ = 1;
+    }
+    else if (condition_status == NodeStatus::FAILURE)
+    {
+      if( children_count == 3){
+        child_idx_ = 2;
+      }
+      else{
+        return condition_status;
+      }
+    }
+  }
+  // not an else
+  if (child_idx_ > 0)
+  {
+    NodeStatus status = children_nodes_[child_idx_]->executeTick();
+    if (status == NodeStatus::RUNNING)
+    {
+      return NodeStatus::RUNNING;
+    }
+    else{
+      haltChildren();
+      child_idx_ = 0;
+      return status;
+    }
+  }
+
+  throw std::logic_error("Something unexpected happened in IfThenElseNode");
+}
+
+}  // namespace BT
diff --git a/src/controls/manual_node.cpp b/src/controls/manual_node.cpp
new file mode 100644 (file)
index 0000000..d27ec98
--- /dev/null
@@ -0,0 +1,215 @@
+/* Copyright (C) 2015-2018 Michele Colledanchise -  All Rights Reserved
+ * Copyright (C) 2018-2020 Davide Faconti, Eurecat -  All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#include "behaviortree_cpp_v3/controls/manual_node.h"
+#include "behaviortree_cpp_v3/action_node.h"
+#include <ncurses.h>
+
+namespace BT
+{
+
+
+ManualSelectorNode::ManualSelectorNode(const std::string& name, const NodeConfiguration& config)
+    : ControlNode::ControlNode(name, config )
+  , running_child_idx_(-1)
+  , previously_executed_idx_(-1)
+{
+    setRegistrationID("ManualSelector");
+}
+
+void ManualSelectorNode::halt()
+{
+    if( running_child_idx_ >= 0 )
+    {
+        haltChild( size_t(running_child_idx_) );
+    }
+    running_child_idx_ = -1;
+    ControlNode::halt();
+}
+
+NodeStatus ManualSelectorNode::tick()
+{
+    const size_t children_count = children_nodes_.size();
+
+    if( children_count == 0 )
+    {
+        return selectStatus();
+    }
+
+    bool repeat_last = false;
+    getInput(REPEAT_LAST_SELECTION, repeat_last);
+
+    int idx = 0;
+
+    if( repeat_last && previously_executed_idx_ >= 0)
+    {
+        idx = previously_executed_idx_;
+    }
+    else{
+        setStatus(NodeStatus::RUNNING);
+        idx = selectChild();
+        previously_executed_idx_ = idx;
+
+        if( idx == NUM_SUCCESS ){
+            return NodeStatus::SUCCESS;
+        }
+        if( idx == NUM_FAILURE ){
+            return NodeStatus::FAILURE;
+        }
+        if( idx == NUM_RUNNING ){
+            return NodeStatus::RUNNING;
+        }
+    }
+
+    NodeStatus ret = children_nodes_[idx]->executeTick();
+    if(ret == NodeStatus::RUNNING)
+    {
+        running_child_idx_ = idx;
+    }
+    return ret;
+}
+
+NodeStatus ManualSelectorNode::selectStatus() const
+{
+    WINDOW *win;
+    initscr();
+    cbreak();
+
+    win = newwin( 6, 70, 1, 1 ); // create a new window
+
+    mvwprintw( win, 0, 0, "No children." );
+    mvwprintw( win, 1, 0, "Press: S to return SUCCESFULL," );
+    mvwprintw( win, 2, 0, "       F to return FAILURE, or" );
+    mvwprintw( win, 3, 0, "       R to return RUNNING." );
+
+    wrefresh( win ); // update the terminal screen
+    noecho(); // disable echoing of characters on the screen
+    keypad( win, TRUE ); // enable keyboard input for the window.
+    curs_set( 0 ); // hide the default screen cursor.
+
+    int ch = 0;
+    NodeStatus ret;
+    while(1)
+    {
+        if( ch == 's' || ch == 'S')
+        {
+            ret = NodeStatus::SUCCESS;
+            break;
+        }
+        else if( ch == 'f' || ch == 'F')
+        {
+            ret = NodeStatus::FAILURE;
+            break;
+        }
+        else if( ch == 'r' || ch == 'R')
+        {
+            ret = NodeStatus::RUNNING;
+            break;
+        }
+        ch = wgetch(win);
+    }
+    werase( win ) ;
+    wrefresh( win );
+    delwin( win );
+    endwin();
+    return ret;
+}
+
+uint8_t ManualSelectorNode::selectChild() const
+{
+    const size_t children_count = children_nodes_.size();
+
+    std::vector<std::string> list;
+    list.reserve(children_count);
+    for(const auto& child: children_nodes_)
+    {
+        list.push_back(child->name());
+    }
+
+    size_t width = 10;
+    for(const auto& str: list) {
+        width = std::max(width, str.size()+2);
+    }
+
+    WINDOW *win;
+    initscr();
+    cbreak();
+
+    win = newwin( children_count+6, 70, 1, 1 ); // create a new window
+
+    mvwprintw( win, 0, 0, "Use UP/DOWN arrow to select the child, Enter to confirm." );
+    mvwprintw( win, 1, 0, "Press: S to skip and return SUCCESFULL," );
+    mvwprintw( win, 2, 0, "       F to skip and return FAILURE, or" );
+    mvwprintw( win, 3, 0, "       R to skip and return RUNNING." );
+
+    // now print all the menu items and highlight the first one
+    for(size_t i=0; i<list.size(); i++ )
+    {
+        mvwprintw( win, i+5, 0, "%2d. %s", i+1, list[i].c_str() );
+    }
+
+    wrefresh( win ); // update the terminal screen
+    noecho(); // disable echoing of characters on the screen
+    keypad( win, TRUE ); // enable keyboard input for the window.
+    curs_set( 0 ); // hide the default screen cursor.
+
+    uint8_t row = 0;
+    int ch=0;
+    while(1)
+    {
+        // right pad with spaces to make the items appear with even width.
+        wattroff( win, A_STANDOUT );
+        mvwprintw( win, row+5, 4, "%s", list[row].c_str() );
+        // use a variable to increment or decrement the value based on the input.
+        if( ch == KEY_DOWN )
+        {
+            row = (row == children_count-1) ? 0 : row+1;
+        }
+        else if( ch == KEY_UP )
+        {
+            row = ( row == 0) ? (children_count-1) : row-1;
+        }
+        else if( ch == KEY_ENTER || ch == 10 )
+        {
+            break;
+        }
+        else if( ch == 's' || ch == 'S')
+        {
+            row = NUM_SUCCESS;
+            break;
+        }
+        else if( ch == 'f' || ch == 'F')
+        {
+            row = NUM_FAILURE;
+            break;
+        }
+        else if( ch == 'r' || ch == 'R')
+        {
+            row = NUM_RUNNING;
+            break;
+        }
+
+        // now highlight the next item in the list.
+        wattron( win, A_STANDOUT );
+        mvwprintw( win, row+5, 4, "%s", list[row].c_str());
+        ch = wgetch(win);
+    };
+
+    werase( win ) ;
+    wrefresh( win );
+    delwin( win );
+    endwin();
+    return row;
+}
+
+}
diff --git a/src/controls/parallel_node.cpp b/src/controls/parallel_node.cpp
new file mode 100644 (file)
index 0000000..18ff9e7
--- /dev/null
@@ -0,0 +1,165 @@
+/* Copyright (C) 2015-2018 Michele Colledanchise -  All Rights Reserved
+ * Copyright (C) 2018-2020 Davide Faconti, Eurecat -  All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#include "behaviortree_cpp_v3/controls/parallel_node.h"
+
+namespace BT
+{
+
+constexpr const char* ParallelNode::THRESHOLD_FAILURE;
+constexpr const char* ParallelNode::THRESHOLD_SUCCESS;
+
+ParallelNode::ParallelNode(const std::string& name, unsigned success_threshold,
+                           unsigned failure_threshold)
+    : ControlNode::ControlNode(name, {} ),
+    success_threshold_(success_threshold),
+    failure_threshold_(failure_threshold),
+    read_parameter_from_ports_(false)
+{
+    setRegistrationID("Parallel");
+}
+
+ParallelNode::ParallelNode(const std::string &name,
+                               const NodeConfiguration& config)
+    : ControlNode::ControlNode(name, config),
+      success_threshold_(1),
+      failure_threshold_(1),
+      read_parameter_from_ports_(true)
+{
+}
+
+NodeStatus ParallelNode::tick()
+{
+    if(read_parameter_from_ports_)
+    {
+        if( !getInput(THRESHOLD_SUCCESS, success_threshold_) )
+        {
+            throw RuntimeError("Missing parameter [", THRESHOLD_SUCCESS, "] in ParallelNode");
+        }
+
+        if( !getInput(THRESHOLD_FAILURE, failure_threshold_) )
+        {
+            throw RuntimeError("Missing parameter [", THRESHOLD_FAILURE, "] in ParallelNode");
+        }
+    }
+
+    size_t success_childred_num = 0;
+    size_t failure_childred_num = 0;
+
+    const size_t children_count = children_nodes_.size();
+
+    if( children_count < success_threshold_)
+    {
+        throw LogicError("Number of children is less than threshold. Can never succeed.");
+    }
+
+    if( children_count < failure_threshold_)
+    {
+        throw LogicError("Number of children is less than threshold. Can never fail.");
+    }
+
+    // Routing the tree according to the sequence node's logic:
+    for (unsigned int i = 0; i < children_count; i++)
+    {
+        TreeNode* child_node = children_nodes_[i];
+
+        bool in_skip_list = (skip_list_.count(i) != 0);
+
+        NodeStatus child_status;
+        if( in_skip_list )
+        {
+            child_status = child_node->status();
+        }
+        else {
+            child_status = child_node->executeTick();
+        }
+
+        switch (child_status)
+        {
+            case NodeStatus::SUCCESS:
+            {
+                if( !in_skip_list )
+                {
+                    skip_list_.insert(i);
+                }
+                success_childred_num++;
+
+                if (success_childred_num == success_threshold_)
+                {
+                    skip_list_.clear();
+                    haltChildren();
+                    return NodeStatus::SUCCESS;
+                }
+            } break;
+
+            case NodeStatus::FAILURE:
+            {
+                if( !in_skip_list )
+                {
+                    skip_list_.insert(i);
+                }
+                failure_childred_num++;
+                
+                // It fails if it is not possible to succeed anymore or if 
+                // number of failures are equal to failure_threshold_
+                if ((failure_childred_num > children_count - success_threshold_)
+                    || (failure_childred_num == failure_threshold_))
+                {
+                    skip_list_.clear();
+                    haltChildren();
+                    return NodeStatus::FAILURE;
+                }
+            } break;
+
+            case NodeStatus::RUNNING:
+            {
+                // do nothing
+            }  break;
+
+            default:
+            {
+                throw LogicError("A child node must never return IDLE");
+            }
+        }
+    }
+
+    return NodeStatus::RUNNING;
+}
+
+void ParallelNode::halt()
+{
+    skip_list_.clear();
+    ControlNode::halt();
+}
+
+unsigned int ParallelNode::thresholdM()
+{
+    return success_threshold_;
+}
+
+unsigned int ParallelNode::thresholdFM()
+{
+    return failure_threshold_;
+}
+
+void ParallelNode::setThresholdM(unsigned int threshold_M)
+{
+    success_threshold_ = threshold_M;
+}
+
+void ParallelNode::setThresholdFM(unsigned int threshold_M)
+{
+    failure_threshold_ = threshold_M;
+}
+
+}
diff --git a/src/controls/reactive_fallback.cpp b/src/controls/reactive_fallback.cpp
new file mode 100644 (file)
index 0000000..98536d4
--- /dev/null
@@ -0,0 +1,66 @@
+/* Copyright (C) 2020 Davide Faconti, Eurecat -  All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#include "behaviortree_cpp_v3/controls/reactive_fallback.h"
+
+namespace BT
+{
+
+NodeStatus ReactiveFallback::tick()
+{
+    size_t failure_count = 0;
+
+    for (size_t index = 0; index < childrenCount(); index++)
+    {
+        TreeNode* current_child_node = children_nodes_[index];
+        const NodeStatus child_status = current_child_node->executeTick();
+
+        switch (child_status)
+        {
+            case NodeStatus::RUNNING:
+            {
+                for(size_t i=index+1; i < childrenCount(); i++)
+                {
+                    haltChild(i);
+                }
+                return NodeStatus::RUNNING;
+            }
+
+            case NodeStatus::FAILURE:
+            {
+                failure_count++;
+            }break;
+
+            case NodeStatus::SUCCESS:
+            {
+                haltChildren();
+                return NodeStatus::SUCCESS;
+            }
+
+            case NodeStatus::IDLE:
+            {
+                throw LogicError("A child node must never return IDLE");
+            }
+        }   // end switch
+    } //end for
+
+    if( failure_count == childrenCount() )
+    {
+        haltChildren();
+        return NodeStatus::FAILURE;
+    }
+
+    return NodeStatus::RUNNING;
+}
+
+}
+
diff --git a/src/controls/reactive_sequence.cpp b/src/controls/reactive_sequence.cpp
new file mode 100644 (file)
index 0000000..ba6ab50
--- /dev/null
@@ -0,0 +1,67 @@
+/* Copyright (C) 2020 Davide Faconti, Eurecat -  All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#include "behaviortree_cpp_v3/controls/reactive_sequence.h"
+
+namespace BT
+{
+
+NodeStatus ReactiveSequence::tick()
+{
+    size_t success_count = 0;
+    size_t running_count = 0;
+
+    for (size_t index = 0; index < childrenCount(); index++)
+    {
+        TreeNode* current_child_node = children_nodes_[index];
+        const NodeStatus child_status = current_child_node->executeTick();
+
+        switch (child_status)
+        {
+            case NodeStatus::RUNNING:
+            {
+                running_count++;
+
+                for(size_t i=index+1; i < childrenCount(); i++)
+                {
+                    haltChild(i);
+                }
+                return NodeStatus::RUNNING;
+            }
+
+            case NodeStatus::FAILURE:
+            {
+                haltChildren();
+                return NodeStatus::FAILURE;
+            }
+            case NodeStatus::SUCCESS:
+            {
+                success_count++;
+            }break;
+
+            case NodeStatus::IDLE:
+            {
+                throw LogicError("A child node must never return IDLE");
+            }
+        }   // end switch
+    } //end for
+
+    if( success_count == childrenCount())
+    {
+        haltChildren();
+        return NodeStatus::SUCCESS;
+    }
+    return NodeStatus::RUNNING;
+}
+
+
+}
diff --git a/src/controls/sequence_node.cpp b/src/controls/sequence_node.cpp
new file mode 100644 (file)
index 0000000..800319e
--- /dev/null
@@ -0,0 +1,80 @@
+/* Copyright (C) 2015-2018 Michele Colledanchise -  All Rights Reserved
+ * Copyright (C) 2018-2020 Davide Faconti, Eurecat -  All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#include "behaviortree_cpp_v3/controls/sequence_node.h"
+#include "behaviortree_cpp_v3/action_node.h"
+
+namespace BT
+{
+
+
+SequenceNode::SequenceNode(const std::string& name)
+    : ControlNode::ControlNode(name, {} )
+  , current_child_idx_(0)
+{
+    setRegistrationID("Sequence");
+}
+
+void SequenceNode::halt()
+{
+    current_child_idx_ = 0;
+    ControlNode::halt();
+}
+
+NodeStatus SequenceNode::tick()
+{
+    const size_t children_count = children_nodes_.size();
+
+    setStatus(NodeStatus::RUNNING);
+
+    while (current_child_idx_ < children_count)
+    {
+        TreeNode* current_child_node = children_nodes_[current_child_idx_];
+        const NodeStatus child_status = current_child_node->executeTick();
+
+        switch (child_status)
+        {
+            case NodeStatus::RUNNING:
+            {
+                return child_status;
+            }
+            case NodeStatus::FAILURE:
+            {
+                // Reset on failure
+                haltChildren();
+                current_child_idx_ = 0;
+                return child_status;
+            }
+            case NodeStatus::SUCCESS:
+            {
+                current_child_idx_++;
+            }
+            break;
+
+            case NodeStatus::IDLE:
+            {
+                throw LogicError("A child node must never return IDLE");
+            }
+        }   // end switch
+    }       // end while loop
+
+    // The entire while loop completed. This means that all the children returned SUCCESS.
+    if (current_child_idx_ == children_count)
+    {
+        haltChildren();
+        current_child_idx_ = 0;
+    }
+    return NodeStatus::SUCCESS;
+}
+
+}
diff --git a/src/controls/sequence_star_node.cpp b/src/controls/sequence_star_node.cpp
new file mode 100644 (file)
index 0000000..7bd10ce
--- /dev/null
@@ -0,0 +1,81 @@
+/* Copyright (C) 2015-2018 Michele Colledanchise -  All Rights Reserved
+ * Copyright (C) 2018-2020 Davide Faconti, Eurecat -  All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#include "behaviortree_cpp_v3/controls/sequence_star_node.h"
+
+namespace BT
+{
+
+SequenceStarNode::SequenceStarNode(const std::string& name)
+    : ControlNode::ControlNode(name, {} )
+  , current_child_idx_(0)
+{
+    setRegistrationID("SequenceStar");
+}
+
+NodeStatus SequenceStarNode::tick()
+{
+    const size_t children_count = children_nodes_.size();
+
+    setStatus(NodeStatus::RUNNING);
+
+    while (current_child_idx_ < children_count)
+    {
+        TreeNode* current_child_node = children_nodes_[current_child_idx_];
+        const NodeStatus child_status = current_child_node->executeTick();
+
+        switch (child_status)
+        {
+            case NodeStatus::RUNNING:
+            {
+                return child_status;
+            }
+            case NodeStatus::FAILURE:
+            {
+                // DO NOT reset current_child_idx_ on failure
+                for(size_t i=current_child_idx_; i < childrenCount(); i++)
+                {
+                    haltChild(i);
+                }
+
+                return child_status;
+            }
+            case NodeStatus::SUCCESS:
+            {
+                current_child_idx_++;
+            }
+            break;
+
+            case NodeStatus::IDLE:
+            {
+                throw LogicError("A child node must never return IDLE");
+            }
+        }   // end switch
+    }       // end while loop
+
+    // The entire while loop completed. This means that all the children returned SUCCESS.
+    if (current_child_idx_ == children_count)
+    {
+        haltChildren();
+        current_child_idx_ = 0;
+    }
+    return NodeStatus::SUCCESS;
+}
+
+void SequenceStarNode::halt()
+{
+    current_child_idx_ = 0;
+    ControlNode::halt();
+}
+
+}
diff --git a/src/controls/switch_node.cpp b/src/controls/switch_node.cpp
new file mode 100644 (file)
index 0000000..2ee4362
--- /dev/null
@@ -0,0 +1,15 @@
+/* Copyright (C) 2015-2018 Michele Colledanchise -  All Rights Reserved
+ * Copyright (C) 2018-2020 Davide Faconti, Eurecat -  All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#include "behaviortree_cpp_v3/controls/switch_node.h"
+#include "behaviortree_cpp_v3/action_node.h"
diff --git a/src/controls/while_do_else_node.cpp b/src/controls/while_do_else_node.cpp
new file mode 100644 (file)
index 0000000..4757e27
--- /dev/null
@@ -0,0 +1,72 @@
+/* Copyright (C) 2020 Davide Faconti -  All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#include "behaviortree_cpp_v3/controls/while_do_else_node.h"
+
+
+namespace BT
+{
+
+WhileDoElseNode::WhileDoElseNode(const std::string &name)
+  : ControlNode::ControlNode(name, {} )
+{
+  setRegistrationID("WhileDoElse");
+}
+
+void WhileDoElseNode::halt()
+{
+  ControlNode::halt();
+}
+
+NodeStatus WhileDoElseNode::tick()
+{
+  const size_t children_count = children_nodes_.size();
+
+  if(children_count != 3)
+  {
+    throw std::logic_error("WhileDoElse must have either 2 or 3 children");
+  }
+
+  setStatus(NodeStatus::RUNNING);
+
+  NodeStatus condition_status = children_nodes_[0]->executeTick();
+
+  if (condition_status == NodeStatus::RUNNING)
+  {
+    return condition_status;
+  }
+
+  NodeStatus status = NodeStatus::IDLE;
+
+  if (condition_status == NodeStatus::SUCCESS)
+  {
+    haltChild(2);
+    status = children_nodes_[1]->executeTick();
+  }
+  else if (condition_status == NodeStatus::FAILURE)
+  {
+    haltChild(1);
+    status = children_nodes_[2]->executeTick();
+  }
+
+  if (status == NodeStatus::RUNNING)
+  {
+    return NodeStatus::RUNNING;
+  }
+  else
+  {
+    haltChildren();
+    return status;
+  }
+}
+
+}  // namespace BT
diff --git a/src/decorator_node.cpp b/src/decorator_node.cpp
new file mode 100644 (file)
index 0000000..6c99676
--- /dev/null
@@ -0,0 +1,80 @@
+/* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved
+ * Copyright (C) 2018-2020 Davide Faconti, Eurecat -  All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#include "behaviortree_cpp_v3/decorator_node.h"
+
+namespace BT
+{
+DecoratorNode::DecoratorNode(const std::string& name, const NodeConfiguration& config)
+  : TreeNode::TreeNode(name, config), child_node_(nullptr)
+{
+}
+
+void DecoratorNode::setChild(TreeNode* child)
+{
+    if (child_node_)
+    {
+        throw BehaviorTreeException("Decorator [", name(), "] has already a child assigned");
+    }
+
+    child_node_ = child;
+}
+
+void DecoratorNode::halt()
+{
+    haltChild();
+    setStatus(NodeStatus::IDLE);
+}
+
+const TreeNode* DecoratorNode::child() const
+{
+    return child_node_;
+}
+
+TreeNode* DecoratorNode::child()
+{
+    return child_node_;
+}
+
+void DecoratorNode::haltChild()
+{
+    if (child_node_->status() == NodeStatus::RUNNING)
+    {
+        child_node_->halt();
+    }
+    child_node_->setStatus(NodeStatus::IDLE);
+}
+
+SimpleDecoratorNode::SimpleDecoratorNode(const std::string& name, TickFunctor tick_functor,
+                                         const NodeConfiguration& config)
+  : DecoratorNode(name, config), tick_functor_(std::move(tick_functor))
+{
+}
+
+NodeStatus SimpleDecoratorNode::tick()
+{
+    return tick_functor_(child()->executeTick(), *this);
+}
+
+NodeStatus DecoratorNode::executeTick()
+{
+    NodeStatus status = TreeNode::executeTick();
+    NodeStatus child_status = child()->status();
+    if( child_status == NodeStatus::SUCCESS || child_status == NodeStatus::FAILURE )
+    {
+        child()->setStatus(NodeStatus::IDLE);
+    }
+    return status;
+}
+
+}
diff --git a/src/decorators/delay_node.cpp b/src/decorators/delay_node.cpp
new file mode 100644 (file)
index 0000000..47b88d5
--- /dev/null
@@ -0,0 +1,79 @@
+/*  Contributed by Indraneel on 26/04/2020
+*/
+#include "behaviortree_cpp_v3/decorators/delay_node.h"
+#include "behaviortree_cpp_v3/action_node.h"
+
+namespace BT
+{
+DelayNode::DelayNode(const std::string& name, unsigned milliseconds)
+  : DecoratorNode(name, {})
+  , delay_started_(false)
+  , delay_aborted_(false)
+  , msec_(milliseconds)
+  , read_parameter_from_ports_(false)
+{
+    setRegistrationID("Delay");
+}
+
+DelayNode::DelayNode(const std::string& name, const NodeConfiguration& config)
+  : DecoratorNode(name, config)
+  , delay_started_(false)
+  , delay_aborted_(false)
+  , msec_(0)
+  , read_parameter_from_ports_(true)
+{
+}
+
+NodeStatus DelayNode::tick()
+{
+    if (read_parameter_from_ports_)
+    {
+        if (!getInput("delay_msec", msec_))
+        {
+            throw RuntimeError("Missing parameter [delay_msec] in DelayNode");
+        }
+    }
+
+    if (!delay_started_)
+    {
+        delay_complete_ = false;
+        delay_started_ = true;
+        setStatus(NodeStatus::RUNNING);
+
+        timer_id_ = timer_.add(std::chrono::milliseconds(msec_),
+                               [this](bool aborted)
+        {
+            std::unique_lock<std::mutex> lk(delay_mutex_);
+            if (!aborted)
+            {
+                delay_complete_ = true;
+            }
+            else
+            {
+                delay_aborted_ = true;
+            }
+        });
+    }
+
+    std::unique_lock<std::mutex> lk(delay_mutex_);
+
+    if (delay_aborted_)
+    {
+        delay_aborted_ = false;
+        delay_started_ = false;
+        return NodeStatus::FAILURE;
+    }
+    else if (delay_complete_)
+    {
+        delay_started_ = false;
+        delay_aborted_ = false;
+        auto child_status = child()->executeTick();
+        return child_status;
+    }
+    else
+    {
+        return NodeStatus::RUNNING;
+    }
+}
+
+}   // namespace BT
diff --git a/src/decorators/inverter_node.cpp b/src/decorators/inverter_node.cpp
new file mode 100644 (file)
index 0000000..e20e1ed
--- /dev/null
@@ -0,0 +1,55 @@
+/* Copyright (C) 2015-2018 Michele Colledanchise -  All Rights Reserved
+ * Copyright (C) 2018-2020 Davide Faconti, Eurecat -  All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#include "behaviortree_cpp_v3/decorators/inverter_node.h"
+
+namespace BT
+{
+InverterNode::InverterNode(const std::string& name) :
+    DecoratorNode(name, {} )
+{
+    setRegistrationID("Inverter");
+}
+
+NodeStatus InverterNode::tick()
+{
+    setStatus(NodeStatus::RUNNING);
+
+    const NodeStatus child_state = child_node_->executeTick();
+
+    switch (child_state)
+    {
+        case NodeStatus::SUCCESS:
+        {
+            return NodeStatus::FAILURE;
+        }
+
+        case NodeStatus::FAILURE:
+        {
+            return NodeStatus::SUCCESS;
+        }
+
+        case NodeStatus::RUNNING:
+        {
+            return NodeStatus::RUNNING;
+        }
+
+        default:
+        {
+            throw LogicError("A child node must never return IDLE");
+        }
+    }
+    //return status();
+}
+
+}
diff --git a/src/decorators/repeat_node.cpp b/src/decorators/repeat_node.cpp
new file mode 100644 (file)
index 0000000..7a4509a
--- /dev/null
@@ -0,0 +1,92 @@
+/* Copyright (C) 2015-2018 Michele Colledanchise -  All Rights Reserved
+ * Copyright (C) 2018-2020 Davide Faconti, Eurecat -  All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#include "behaviortree_cpp_v3/decorators/repeat_node.h"
+
+namespace BT
+{
+constexpr const char* RepeatNode::NUM_CYCLES;
+
+RepeatNode::RepeatNode(const std::string& name, int NTries)
+    : DecoratorNode(name, {} ),
+    num_cycles_(NTries),
+    try_index_(0),
+    read_parameter_from_ports_(false)
+{
+     setRegistrationID("Repeat");
+}
+
+RepeatNode::RepeatNode(const std::string& name, const NodeConfiguration& config)
+  : DecoratorNode(name, config),
+    num_cycles_(0),
+    try_index_(0),
+    read_parameter_from_ports_(true)
+{
+
+}
+
+NodeStatus RepeatNode::tick()
+{
+    if( read_parameter_from_ports_ )
+    {
+        if( !getInput(NUM_CYCLES, num_cycles_) )
+        {
+            throw RuntimeError("Missing parameter [", NUM_CYCLES, "] in RepeatNode");
+        }
+    }
+
+    setStatus(NodeStatus::RUNNING);
+
+    while (try_index_ < num_cycles_ || num_cycles_== -1 )
+    {
+        NodeStatus child_state = child_node_->executeTick();
+
+        switch (child_state)
+        {
+            case NodeStatus::SUCCESS:
+            {
+                try_index_++;
+                haltChild();
+            }
+            break;
+
+            case NodeStatus::FAILURE:
+            {
+                try_index_ = 0;
+                haltChild();
+                return (NodeStatus::FAILURE);
+            }
+
+            case NodeStatus::RUNNING:
+            {
+                return NodeStatus::RUNNING;
+            }
+
+            default:
+            {
+                throw LogicError("A child node must never return IDLE");
+            }
+        }
+    }
+
+    try_index_ = 0;
+    return NodeStatus::SUCCESS;
+}
+
+void RepeatNode::halt()
+{
+    try_index_ = 0;
+    DecoratorNode::halt();
+}
+
+}
diff --git a/src/decorators/retry_node.cpp b/src/decorators/retry_node.cpp
new file mode 100644 (file)
index 0000000..6f8386d
--- /dev/null
@@ -0,0 +1,90 @@
+/* Copyright (C) 2015-2018 Michele Colledanchise -  All Rights Reserved
+ * Copyright (C) 2018-2020 Davide Faconti, Eurecat -  All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#include "behaviortree_cpp_v3/decorators/retry_node.h"
+
+namespace BT
+{
+constexpr const char* RetryNode::NUM_ATTEMPTS;
+
+RetryNode::RetryNode(const std::string& name, int NTries)
+    : DecoratorNode(name, {} ),
+    max_attempts_(NTries),
+    try_index_(0),
+    read_parameter_from_ports_(false)
+{
+    setRegistrationID("RetryUntilSuccessful");
+}
+
+RetryNode::RetryNode(const std::string& name, const NodeConfiguration& config)
+  : DecoratorNode(name, config),
+    max_attempts_(0),
+    try_index_(0),
+    read_parameter_from_ports_(true)
+{
+}
+
+void RetryNode::halt()
+{
+    try_index_ = 0;
+    DecoratorNode::halt();
+}
+
+NodeStatus RetryNode::tick()
+{
+    if( read_parameter_from_ports_ )
+    {
+        if( !getInput(NUM_ATTEMPTS, max_attempts_) )
+        {
+            throw RuntimeError("Missing parameter [", NUM_ATTEMPTS,"] in RetryNode");
+        }
+    }
+
+    setStatus(NodeStatus::RUNNING);
+
+    while (try_index_ < max_attempts_ || max_attempts_ == -1)
+    {
+        NodeStatus child_state = child_node_->executeTick();
+        switch (child_state)
+        {
+            case NodeStatus::SUCCESS:
+            {
+                try_index_ = 0;
+                haltChild();
+                return (NodeStatus::SUCCESS);
+            }
+
+            case NodeStatus::FAILURE:
+            {
+                try_index_++;
+                haltChild();
+            }
+            break;
+
+            case NodeStatus::RUNNING:
+            {
+                return NodeStatus::RUNNING;
+            }
+
+            default:
+            {
+                throw LogicError("A child node must never return IDLE");
+            }
+        }
+    }
+
+    try_index_ = 0;
+    return NodeStatus::FAILURE;
+}
+
+}
diff --git a/src/decorators/subtree_node.cpp b/src/decorators/subtree_node.cpp
new file mode 100644 (file)
index 0000000..20ffaf5
--- /dev/null
@@ -0,0 +1,37 @@
+#include "behaviortree_cpp_v3/decorators/subtree_node.h"
+
+
+BT::SubtreeNode::SubtreeNode(const std::string &name) :
+    DecoratorNode(name, {} )
+{
+    setRegistrationID("SubTree");
+}
+
+BT::NodeStatus BT::SubtreeNode::tick()
+{
+    NodeStatus prev_status = status();
+    if (prev_status == NodeStatus::IDLE)
+    {
+        setStatus(NodeStatus::RUNNING);
+    }
+    return child_node_->executeTick();
+}
+
+
+//--------------------------------
+BT::SubtreePlusNode::SubtreePlusNode(const std::string &name) :
+     DecoratorNode(name, {} )
+{
+  setRegistrationID("SubTreePlus");
+}
+
+BT::NodeStatus BT::SubtreePlusNode::tick()
+{
+    NodeStatus prev_status = status();
+    if (prev_status == NodeStatus::IDLE)
+    {
+        setStatus(NodeStatus::RUNNING);
+    }
+    return child_node_->executeTick();
+}
+
diff --git a/src/example.cpp b/src/example.cpp
new file mode 100644 (file)
index 0000000..e430603
--- /dev/null
@@ -0,0 +1,80 @@
+#include <iostream>
+#include <behavior_tree.h>
+
+class MyCondition : public BT::ConditionNode
+{
+  public:
+    MyCondition(const std::string& name);
+    ~MyCondition();
+    BT::ReturnStatus Tick();
+};
+
+MyCondition::MyCondition(const std::string& name) : BT::ConditionNode::ConditionNode(name)
+{
+}
+
+BT::ReturnStatus MyCondition::Tick()
+{
+    std::cout << "The Condition is true" << std::endl;
+
+    return NodeStatus::SUCCESS;
+}
+
+class MyAction : public BT::ActionNode
+{
+  public:
+    MyAction(const std::string& name);
+    ~MyAction();
+    BT::ReturnStatus Tick();
+    void Halt();
+};
+
+MyAction::MyAction(const std::string& name) : ActionNode::ActionNode(name)
+{
+}
+
+BT::ReturnStatus MyAction::Tick()
+{
+    std::cout << "The Action is doing some operations" << std::endl;
+    std::this_thread::sleep_for(std::chrono::milliseconds(500));
+    if (is_halted())
+    {
+        return NodeStatus::IDLE;
+    }
+
+    std::cout << "The Action is doing some others operations" << std::endl;
+    std::this_thread::sleep_for(std::chrono::milliseconds(500));
+    if (is_halted())
+    {
+        return NodeStatus::IDLE;
+    }
+
+    std::cout << "The Action is doing more operations" << std::endl;
+    std::this_thread::sleep_for(std::chrono::milliseconds(500));
+    if (is_halted())
+    {
+        return NodeStatus::IDLE;
+    }
+
+    std::cout << "The Action has succeeded" << std::endl;
+    return NodeStatus::SUCCESS;
+}
+
+void MyAction::Halt()
+{
+}
+
+int main(int argc, char* argv[])
+{
+    BT::SequenceNode* seq = new BT::SequenceNode("Sequence");
+    MyCondition* my_con_1 = new MyCondition("Condition");
+    MyAction* my_act_1 = new MyAction("Action");
+    int tick_time_milliseconds = 1000;
+
+    seq->AddChild(my_con_1);
+    seq->AddChild(my_act_1);
+
+    Execute(seq, tick_time_milliseconds);
+
+    return 0;
+}
diff --git a/src/loggers/bt_cout_logger.cpp b/src/loggers/bt_cout_logger.cpp
new file mode 100644 (file)
index 0000000..c91da69
--- /dev/null
@@ -0,0 +1,43 @@
+#include "behaviortree_cpp_v3/loggers/bt_cout_logger.h"
+
+namespace BT
+{
+std::atomic<bool> StdCoutLogger::ref_count(false);
+
+StdCoutLogger::StdCoutLogger(const BT::Tree& tree) : StatusChangeLogger(tree.rootNode())
+{
+    bool expected = false;
+    if (!ref_count.compare_exchange_strong(expected, true))
+    {
+        throw LogicError("Only one instance of StdCoutLogger shall be created");
+    }
+}
+StdCoutLogger::~StdCoutLogger()
+{
+    ref_count.store(false);
+}
+
+void StdCoutLogger::callback(Duration timestamp, const TreeNode& node, NodeStatus prev_status,
+                             NodeStatus status)
+{
+    using namespace std::chrono;
+
+    constexpr const char* whitespaces = "                         ";
+    constexpr const size_t ws_count = 25;
+
+    double since_epoch = duration<double>(timestamp).count();
+    printf("[%.3f]: %s%s %s -> %s",
+           since_epoch, node.name().c_str(),
+           &whitespaces[std::min(ws_count, node.name().size())],
+           toStr(prev_status, true).c_str(),
+           toStr(status, true).c_str() );
+    std::cout << std::endl;
+}
+
+void StdCoutLogger::flush()
+{
+    std::cout << std::flush;
+       ref_count = false;
+}
+
+}   // end namespace
diff --git a/src/loggers/bt_file_logger.cpp b/src/loggers/bt_file_logger.cpp
new file mode 100644 (file)
index 0000000..fd83518
--- /dev/null
@@ -0,0 +1,66 @@
+#include "behaviortree_cpp_v3/loggers/bt_file_logger.h"
+#include "behaviortree_cpp_v3/flatbuffers/bt_flatbuffer_helper.h"
+
+namespace BT
+{
+FileLogger::FileLogger(const BT::Tree& tree, const char* filename, uint16_t buffer_size)
+  : StatusChangeLogger(tree.rootNode()), buffer_max_size_(buffer_size)
+{
+    if (buffer_max_size_ != 0)
+    {
+        buffer_.reserve(buffer_max_size_);
+    }
+
+    enableTransitionToIdle(true);
+
+    flatbuffers::FlatBufferBuilder builder(1024);
+    CreateFlatbuffersBehaviorTree(builder, tree);
+
+    //-------------------------------------
+
+    file_os_.open(filename, std::ofstream::binary | std::ofstream::out);
+
+    // serialize the length of the buffer in the first 4 bytes
+    char size_buff[4];
+    flatbuffers::WriteScalar(size_buff, static_cast<int32_t>(builder.GetSize()));
+
+    file_os_.write(size_buff, 4);
+    file_os_.write(reinterpret_cast<const char*>(builder.GetBufferPointer()), builder.GetSize());
+}
+
+FileLogger::~FileLogger()
+{
+    this->flush();
+    file_os_.close();
+}
+
+void FileLogger::callback(Duration timestamp, const TreeNode& node, NodeStatus prev_status,
+                          NodeStatus status)
+{
+    SerializedTransition buffer =
+        SerializeTransition(node.UID(), timestamp, prev_status, status);
+
+    if (buffer_max_size_ == 0)
+    {
+        file_os_.write(reinterpret_cast<const char*>(buffer.data()), buffer.size());
+    }
+    else
+    {
+        buffer_.push_back(buffer);
+        if (buffer_.size() >= buffer_max_size_)
+        {
+            this->flush();
+        }
+    }
+}
+
+void FileLogger::flush()
+{
+    for (const auto& array : buffer_)
+    {
+        file_os_.write(reinterpret_cast<const char*>(array.data()), array.size());
+    }
+    file_os_.flush();
+    buffer_.clear();
+}
+}
diff --git a/src/loggers/bt_minitrace_logger.cpp b/src/loggers/bt_minitrace_logger.cpp
new file mode 100644 (file)
index 0000000..d97d772
--- /dev/null
@@ -0,0 +1,78 @@
+
+#include "behaviortree_cpp_v3/loggers/bt_minitrace_logger.h"
+#include "minitrace/minitrace.h"
+
+namespace BT
+{
+std::atomic<bool> MinitraceLogger::ref_count(false);
+
+MinitraceLogger::MinitraceLogger(const Tree &tree, const char* filename_json)
+  : StatusChangeLogger( tree.rootNode() )
+{
+    bool expected = false;
+    if (!ref_count.compare_exchange_strong(expected, true))
+    {
+        throw LogicError("Only one instance of StdCoutLogger shall be created");
+    }
+
+    minitrace::mtr_register_sigint_handler();
+    minitrace::mtr_init(filename_json);
+    this->enableTransitionToIdle(true);
+}
+
+MinitraceLogger::~MinitraceLogger()
+{
+    minitrace::mtr_flush();
+    minitrace::mtr_shutdown();
+    ref_count = false;
+}
+
+const char* toConstStr(NodeType type)
+{
+  switch (type)
+  {
+    case NodeType::ACTION:
+      return "Action";
+    case NodeType::CONDITION:
+      return "Condition";
+    case NodeType::DECORATOR:
+      return "Decorator";
+    case NodeType::CONTROL:
+      return "Control";
+    case NodeType::SUBTREE:
+      return "SubTree";
+    default:
+      return "Undefined";
+  }
+}
+
+void MinitraceLogger::callback(Duration /*timestamp*/,
+                               const TreeNode& node, NodeStatus prev_status,
+                               NodeStatus status)
+{
+    using namespace minitrace;
+
+    const bool statusCompleted = (status == NodeStatus::SUCCESS || status == NodeStatus::FAILURE);
+
+    const char* category = toConstStr(node.type());
+    const char* name = node.name().c_str();
+
+    if (prev_status == NodeStatus::IDLE && statusCompleted)
+    {
+        MTR_INSTANT(category, name);
+    }
+    else if (status == NodeStatus::RUNNING)
+    {
+        MTR_BEGIN(category, name);
+    }
+    else if (prev_status == NodeStatus::RUNNING && statusCompleted)
+    {
+        MTR_END(category, name);
+    }
+}
+
+void MinitraceLogger::flush()
+{
+    minitrace::mtr_flush();
+}
+}   // end namespace
diff --git a/src/loggers/bt_zmq_publisher.cpp b/src/loggers/bt_zmq_publisher.cpp
new file mode 100644 (file)
index 0000000..9f04223
--- /dev/null
@@ -0,0 +1,185 @@
+#include "behaviortree_cpp_v3/loggers/bt_zmq_publisher.h"
+#include "behaviortree_cpp_v3/flatbuffers/bt_flatbuffer_helper.h"
+#include <future>
+#include "zmq.hpp"
+
+namespace BT
+{
+std::atomic<bool> PublisherZMQ::ref_count(false);
+
+struct PublisherZMQ::Pimpl
+{
+    Pimpl():
+        context(1)
+      , publisher(context, ZMQ_PUB)
+      , server(context, ZMQ_REP)
+    {}
+
+    zmq::context_t context;
+    zmq::socket_t publisher;
+    zmq::socket_t server;
+};
+
+
+PublisherZMQ::PublisherZMQ(const BT::Tree& tree,
+                           unsigned max_msg_per_second,
+                           unsigned publisher_port,
+                           unsigned server_port)
+  : StatusChangeLogger(tree.rootNode())
+  , tree_(tree)
+  , min_time_between_msgs_(std::chrono::microseconds(1000 * 1000) / max_msg_per_second)
+  , send_pending_(false)
+  , zmq_(new Pimpl())
+{
+    bool expected = false;
+    if (!ref_count.compare_exchange_strong(expected, true))
+    {
+        throw LogicError("Only one instance of PublisherZMQ shall be created");
+    }
+    if( publisher_port == server_port)
+    {
+        throw LogicError("The TCP ports of the publisher and the server must be different");
+    }
+
+    flatbuffers::FlatBufferBuilder builder(1024);
+    CreateFlatbuffersBehaviorTree(builder, tree);
+
+    tree_buffer_.resize(builder.GetSize());
+    memcpy(tree_buffer_.data(), builder.GetBufferPointer(), builder.GetSize());
+
+    char str[100];
+
+    sprintf(str, "tcp://*:%d", publisher_port);
+    zmq_->publisher.bind(str);
+    sprintf(str, "tcp://*:%d", server_port);
+    zmq_->server.bind(str);
+
+    int timeout_ms = 100;
+    zmq_->server.set(zmq::sockopt::rcvtimeo, timeout_ms);
+
+    active_server_ = true;
+
+    thread_ = std::thread([this]() {
+        while (active_server_)
+        {
+            zmq::message_t req;
+            try
+            {
+                zmq::recv_result_t received = zmq_->server.recv(req);
+                if (received)
+                {
+                    zmq::message_t reply(tree_buffer_.size());
+                    memcpy(reply.data(), tree_buffer_.data(), tree_buffer_.size());
+                    zmq_->server.send(reply, zmq::send_flags::none);
+                }
+            }
+            catch (zmq::error_t& err)
+            {
+                if (err.num() == ETERM)
+                {
+                    std::cout << "[PublisherZMQ] Server quitting." << std::endl;
+                }
+                std::cout << "[PublisherZMQ] just died. Exeption " << err.what() << std::endl;
+                active_server_ = false;
+            }
+        }
+    });
+
+    createStatusBuffer();
+}
+
+PublisherZMQ::~PublisherZMQ()
+{
+    active_server_ = false;
+    zmq_->context.shutdown();
+    if (thread_.joinable())
+    {
+        thread_.join();
+    }
+    flush();
+    delete zmq_;
+    ref_count = false;
+}
+
+
+void PublisherZMQ::createStatusBuffer()
+{
+    status_buffer_.clear();
+    applyRecursiveVisitor(tree_.rootNode(), [this](TreeNode* node) {
+        size_t index = status_buffer_.size();
+        status_buffer_.resize(index + 3);
+        flatbuffers::WriteScalar<uint16_t>(&status_buffer_[index], node->UID());
+        flatbuffers::WriteScalar<int8_t>(&status_buffer_[index + 2],
+                                         static_cast<int8_t>(convertToFlatbuffers(node->status())));
+    });
+}
+
+void PublisherZMQ::callback(Duration timestamp, const TreeNode& node, NodeStatus prev_status,
+                            NodeStatus status)
+{
+    using namespace std::chrono;
+
+    SerializedTransition transition =
+        SerializeTransition(node.UID(), timestamp, prev_status, status);
+    {
+        std::unique_lock<std::mutex> lock(mutex_);
+        transition_buffer_.push_back(transition);
+    }
+
+    if (!send_pending_)
+    {
+        send_pending_ = true;
+        send_future_ = std::async(std::launch::async, [this]() {
+            std::this_thread::sleep_for(min_time_between_msgs_);
+            flush();
+        });
+    }
+}
+
+void PublisherZMQ::flush()
+{
+    zmq::message_t message;
+    {
+        std::unique_lock<std::mutex> lock(mutex_);
+
+        const size_t msg_size = status_buffer_.size() + 8 + (transition_buffer_.size() * 12);
+
+        message.rebuild(msg_size);
+        uint8_t* data_ptr = static_cast<uint8_t*>(message.data());
+
+        // first 4 bytes are the side of the header
+        flatbuffers::WriteScalar<uint32_t>(data_ptr, static_cast<uint32_t>(status_buffer_.size()));
+        data_ptr += sizeof(uint32_t);
+        // copy the header part
+        memcpy(data_ptr, status_buffer_.data(), status_buffer_.size());
+        data_ptr += status_buffer_.size();
+
+        // first 4 bytes are the side of the transition buffer
+        flatbuffers::WriteScalar<uint32_t>(data_ptr, static_cast<uint32_t>(transition_buffer_.size()));
+        data_ptr += sizeof(uint32_t);
+
+        for (auto& transition : transition_buffer_)
+        {
+            memcpy(data_ptr, transition.data(), transition.size());
+            data_ptr += transition.size();
+        }
+        transition_buffer_.clear();
+        createStatusBuffer();
+    }
+    try
+    {
+        zmq_->publisher.send(message, zmq::send_flags::none);
+    }
+    catch (zmq::error_t& err)
+    {
+        if (err.num() == ETERM)
+        {
+            std::cout << "[PublisherZMQ] Publisher quitting." << std::endl;
+        }
+        std::cout << "[PublisherZMQ] just died. Exeption " << err.what() << std::endl;
+    }
+    
+    send_pending_ = false;
+    // printf("%.3f zmq send\n", std::chrono::duration<double>( std::chrono::high_resolution_clock::now().time_since_epoch() ).count());
+}
+}
diff --git a/src/loggers/zmq.hpp b/src/loggers/zmq.hpp
new file mode 100644 (file)
index 0000000..d02a208
--- /dev/null
@@ -0,0 +1,2688 @@
+/*
+    Copyright (c) 2016-2017 ZeroMQ community
+    Copyright (c) 2009-2011 250bpm s.r.o.
+    Copyright (c) 2011 Botond Ballo
+    Copyright (c) 2007-2009 iMatix Corporation
+
+    Permission is hereby granted, free of charge, to any person obtaining a copy
+    of this software and associated documentation files (the "Software"), to
+    deal in the Software without restriction, including without limitation the
+    rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
+    sell copies of the Software, and to permit persons to whom the Software is
+    furnished to do so, subject to the following conditions:
+
+    The above copyright notice and this permission notice shall be included in
+    all copies or substantial portions of the Software.
+
+    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+    FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
+    IN THE SOFTWARE.
+*/
+
+#ifndef __ZMQ_HPP_INCLUDED__
+#define __ZMQ_HPP_INCLUDED__
+
+#ifdef _WIN32
+#ifndef NOMINMAX
+#define NOMINMAX
+#endif
+#endif
+
+// included here for _HAS_CXX* macros
+#include <zmq.h>
+
+#if defined(_MSVC_LANG)
+#define CPPZMQ_LANG _MSVC_LANG
+#else
+#define CPPZMQ_LANG __cplusplus
+#endif
+// overwrite if specific language macros indicate higher version
+#if defined(_HAS_CXX14) && _HAS_CXX14 && CPPZMQ_LANG < 201402L
+#undef CPPZMQ_LANG
+#define CPPZMQ_LANG 201402L
+#endif
+#if defined(_HAS_CXX17) && _HAS_CXX17 && CPPZMQ_LANG < 201703L
+#undef CPPZMQ_LANG
+#define CPPZMQ_LANG 201703L
+#endif
+
+// macros defined if has a specific standard or greater
+#if CPPZMQ_LANG >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1900)
+#define ZMQ_CPP11
+#endif
+#if CPPZMQ_LANG >= 201402L
+#define ZMQ_CPP14
+#endif
+#if CPPZMQ_LANG >= 201703L
+#define ZMQ_CPP17
+#endif
+
+#if defined(ZMQ_CPP14) && !defined(_MSC_VER)
+#define ZMQ_DEPRECATED(msg) [[deprecated(msg)]]
+#elif defined(_MSC_VER)
+#define ZMQ_DEPRECATED(msg) __declspec(deprecated(msg))
+#elif defined(__GNUC__)
+#define ZMQ_DEPRECATED(msg) __attribute__((deprecated(msg)))
+#endif
+
+#if defined(ZMQ_CPP17)
+#define ZMQ_NODISCARD [[nodiscard]]
+#else
+#define ZMQ_NODISCARD
+#endif
+
+#if defined(ZMQ_CPP11)
+#define ZMQ_NOTHROW noexcept
+#define ZMQ_EXPLICIT explicit
+#define ZMQ_OVERRIDE override
+#define ZMQ_NULLPTR nullptr
+#define ZMQ_CONSTEXPR_FN constexpr
+#define ZMQ_CONSTEXPR_VAR constexpr
+#define ZMQ_CPP11_DEPRECATED(msg) ZMQ_DEPRECATED(msg)
+#else
+#define ZMQ_NOTHROW throw()
+#define ZMQ_EXPLICIT
+#define ZMQ_OVERRIDE
+#define ZMQ_NULLPTR 0
+#define ZMQ_CONSTEXPR_FN
+#define ZMQ_CONSTEXPR_VAR const
+#define ZMQ_CPP11_DEPRECATED(msg)
+#endif
+#if defined(ZMQ_CPP14) && (!defined(_MSC_VER) || _MSC_VER > 1900)
+#define ZMQ_EXTENDED_CONSTEXPR
+#endif
+#if defined(ZMQ_CPP17)
+#define ZMQ_INLINE_VAR inline
+#else
+#define ZMQ_INLINE_VAR
+#endif
+
+#include <cassert>
+#include <cstring>
+
+#include <algorithm>
+#include <exception>
+#include <iomanip>
+#include <sstream>
+#include <string>
+#include <vector>
+#ifdef ZMQ_CPP11
+#include <array>
+#include <chrono>
+#include <tuple>
+#include <memory>
+#endif
+
+#if defined(__has_include) && defined(ZMQ_CPP17)
+#define CPPZMQ_HAS_INCLUDE_CPP17(X) __has_include(X)
+#else
+#define CPPZMQ_HAS_INCLUDE_CPP17(X) 0
+#endif
+
+#if CPPZMQ_HAS_INCLUDE_CPP17(<optional>) && !defined(CPPZMQ_HAS_OPTIONAL)
+#define CPPZMQ_HAS_OPTIONAL 1
+#endif
+#ifndef CPPZMQ_HAS_OPTIONAL
+#define CPPZMQ_HAS_OPTIONAL 0
+#elif CPPZMQ_HAS_OPTIONAL
+#include <optional>
+#endif
+
+#if CPPZMQ_HAS_INCLUDE_CPP17(<string_view>) && !defined(CPPZMQ_HAS_STRING_VIEW)
+#define CPPZMQ_HAS_STRING_VIEW 1
+#endif
+#ifndef CPPZMQ_HAS_STRING_VIEW
+#define CPPZMQ_HAS_STRING_VIEW 0
+#elif CPPZMQ_HAS_STRING_VIEW
+#include <string_view>
+#endif
+
+/*  Version macros for compile-time API version detection                     */
+#define CPPZMQ_VERSION_MAJOR 4
+#define CPPZMQ_VERSION_MINOR 7
+#define CPPZMQ_VERSION_PATCH 1
+
+#define CPPZMQ_VERSION                                                              \
+    ZMQ_MAKE_VERSION(CPPZMQ_VERSION_MAJOR, CPPZMQ_VERSION_MINOR,                    \
+                     CPPZMQ_VERSION_PATCH)
+
+//  Detect whether the compiler supports C++11 rvalue references.
+#if (defined(__GNUC__) && (__GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ > 2))   \
+     && defined(__GXX_EXPERIMENTAL_CXX0X__))
+#define ZMQ_HAS_RVALUE_REFS
+#define ZMQ_DELETED_FUNCTION = delete
+#elif defined(__clang__)
+#if __has_feature(cxx_rvalue_references)
+#define ZMQ_HAS_RVALUE_REFS
+#endif
+
+#if __has_feature(cxx_deleted_functions)
+#define ZMQ_DELETED_FUNCTION = delete
+#else
+#define ZMQ_DELETED_FUNCTION
+#endif
+#elif defined(_MSC_VER) && (_MSC_VER >= 1900)
+#define ZMQ_HAS_RVALUE_REFS
+#define ZMQ_DELETED_FUNCTION = delete
+#elif defined(_MSC_VER) && (_MSC_VER >= 1600)
+#define ZMQ_HAS_RVALUE_REFS
+#define ZMQ_DELETED_FUNCTION
+#else
+#define ZMQ_DELETED_FUNCTION
+#endif
+
+#if defined(ZMQ_CPP11) && !defined(__llvm__) && !defined(__INTEL_COMPILER)          \
+  && defined(__GNUC__) && __GNUC__ < 5
+#define ZMQ_CPP11_PARTIAL
+#elif defined(__GLIBCXX__) && __GLIBCXX__ < 20160805
+//the date here is the last date of gcc 4.9.4, which
+// effectively means libstdc++ from gcc 5.5 and higher won't trigger this branch
+#define ZMQ_CPP11_PARTIAL
+#endif
+
+#ifdef ZMQ_CPP11
+#ifdef ZMQ_CPP11_PARTIAL
+#define ZMQ_IS_TRIVIALLY_COPYABLE(T) __has_trivial_copy(T)
+#else
+#include <type_traits>
+#define ZMQ_IS_TRIVIALLY_COPYABLE(T) std::is_trivially_copyable<T>::value
+#endif
+#endif
+
+#if ZMQ_VERSION >= ZMQ_MAKE_VERSION(3, 3, 0)
+#define ZMQ_NEW_MONITOR_EVENT_LAYOUT
+#endif
+
+#if ZMQ_VERSION >= ZMQ_MAKE_VERSION(4, 1, 0)
+#define ZMQ_HAS_PROXY_STEERABLE
+/*  Socket event data  */
+typedef struct
+{
+    uint16_t event; // id of the event as bitfield
+    int32_t value;  // value is either error code, fd or reconnect interval
+} zmq_event_t;
+#endif
+
+// Avoid using deprecated message receive function when possible
+#if ZMQ_VERSION < ZMQ_MAKE_VERSION(3, 2, 0)
+#define zmq_msg_recv(msg, socket, flags) zmq_recvmsg(socket, msg, flags)
+#endif
+
+
+// In order to prevent unused variable warnings when building in non-debug
+// mode use this macro to make assertions.
+#ifndef NDEBUG
+#define ZMQ_ASSERT(expression) assert(expression)
+#else
+#define ZMQ_ASSERT(expression) (void) (expression)
+#endif
+
+namespace zmq
+{
+#ifdef ZMQ_CPP11
+namespace detail
+{
+namespace ranges
+{
+using std::begin;
+using std::end;
+template<class T> auto begin(T &&r) -> decltype(begin(std::forward<T>(r)))
+{
+    return begin(std::forward<T>(r));
+}
+template<class T> auto end(T &&r) -> decltype(end(std::forward<T>(r)))
+{
+    return end(std::forward<T>(r));
+}
+} // namespace ranges
+
+template<class T> using void_t = void;
+
+template<class Iter>
+using iter_value_t = typename std::iterator_traits<Iter>::value_type;
+
+template<class Range>
+using range_iter_t = decltype(
+  ranges::begin(std::declval<typename std::remove_reference<Range>::type &>()));
+
+template<class Range> using range_value_t = iter_value_t<range_iter_t<Range>>;
+
+template<class T, class = void> struct is_range : std::false_type
+{
+};
+
+template<class T>
+struct is_range<
+  T,
+  void_t<decltype(
+    ranges::begin(std::declval<typename std::remove_reference<T>::type &>())
+    == ranges::end(std::declval<typename std::remove_reference<T>::type &>()))>>
+    : std::true_type
+{
+};
+
+} // namespace detail
+#endif
+
+typedef zmq_free_fn free_fn;
+typedef zmq_pollitem_t pollitem_t;
+
+class error_t : public std::exception
+{
+  public:
+    error_t() ZMQ_NOTHROW : errnum(zmq_errno()) {}
+    explicit error_t(int err) ZMQ_NOTHROW : errnum(err) {}
+    virtual const char *what() const ZMQ_NOTHROW ZMQ_OVERRIDE
+    {
+        return zmq_strerror(errnum);
+    }
+    int num() const ZMQ_NOTHROW { return errnum; }
+
+  private:
+    int errnum;
+};
+
+inline int poll(zmq_pollitem_t *items_, size_t nitems_, long timeout_ = -1)
+{
+    int rc = zmq_poll(items_, static_cast<int>(nitems_), timeout_);
+    if (rc < 0)
+        throw error_t();
+    return rc;
+}
+
+ZMQ_DEPRECATED("from 4.3.1, use poll taking non-const items")
+inline int poll(zmq_pollitem_t const *items_, size_t nitems_, long timeout_ = -1)
+{
+    return poll(const_cast<zmq_pollitem_t *>(items_), nitems_, timeout_);
+}
+
+#ifdef ZMQ_CPP11
+ZMQ_DEPRECATED("from 4.3.1, use poll taking non-const items")
+inline int
+poll(zmq_pollitem_t const *items, size_t nitems, std::chrono::milliseconds timeout)
+{
+    return poll(const_cast<zmq_pollitem_t *>(items), nitems,
+                static_cast<long>(timeout.count()));
+}
+
+ZMQ_DEPRECATED("from 4.3.1, use poll taking non-const items")
+inline int poll(std::vector<zmq_pollitem_t> const &items,
+                std::chrono::milliseconds timeout)
+{
+    return poll(const_cast<zmq_pollitem_t *>(items.data()), items.size(),
+                static_cast<long>(timeout.count()));
+}
+
+ZMQ_DEPRECATED("from 4.3.1, use poll taking non-const items")
+inline int poll(std::vector<zmq_pollitem_t> const &items, long timeout_ = -1)
+{
+    return poll(const_cast<zmq_pollitem_t *>(items.data()), items.size(), timeout_);
+}
+
+inline int
+poll(zmq_pollitem_t *items, size_t nitems, std::chrono::milliseconds timeout)
+{
+    return poll(items, nitems, static_cast<long>(timeout.count()));
+}
+
+inline int poll(std::vector<zmq_pollitem_t> &items,
+                std::chrono::milliseconds timeout)
+{
+    return poll(items.data(), items.size(), static_cast<long>(timeout.count()));
+}
+
+ZMQ_DEPRECATED("from 4.3.1, use poll taking std::chrono instead of long")
+inline int poll(std::vector<zmq_pollitem_t> &items, long timeout_ = -1)
+{
+    return poll(items.data(), items.size(), timeout_);
+}
+
+template<std::size_t SIZE>
+inline int poll(std::array<zmq_pollitem_t, SIZE> &items,
+                std::chrono::milliseconds timeout)
+{
+    return poll(items.data(), items.size(), static_cast<long>(timeout.count()));
+}
+#endif
+
+
+inline void version(int *major_, int *minor_, int *patch_)
+{
+    zmq_version(major_, minor_, patch_);
+}
+
+#ifdef ZMQ_CPP11
+inline std::tuple<int, int, int> version()
+{
+    std::tuple<int, int, int> v;
+    zmq_version(&std::get<0>(v), &std::get<1>(v), &std::get<2>(v));
+    return v;
+}
+
+#if !defined(ZMQ_CPP11_PARTIAL)
+namespace detail
+{
+template<class T> struct is_char_type
+{
+    // true if character type for string literals in C++11
+    static constexpr bool value =
+      std::is_same<T, char>::value || std::is_same<T, wchar_t>::value
+      || std::is_same<T, char16_t>::value || std::is_same<T, char32_t>::value;
+};
+}
+#endif
+
+#endif
+
+class message_t
+{
+  public:
+    message_t() ZMQ_NOTHROW
+    {
+        int rc = zmq_msg_init(&msg);
+        ZMQ_ASSERT(rc == 0);
+    }
+
+    explicit message_t(size_t size_)
+    {
+        int rc = zmq_msg_init_size(&msg, size_);
+        if (rc != 0)
+            throw error_t();
+    }
+
+    template<class ForwardIter> message_t(ForwardIter first, ForwardIter last)
+    {
+        typedef typename std::iterator_traits<ForwardIter>::value_type value_t;
+
+        assert(std::distance(first, last) >= 0);
+        size_t const size_ =
+          static_cast<size_t>(std::distance(first, last)) * sizeof(value_t);
+        int const rc = zmq_msg_init_size(&msg, size_);
+        if (rc != 0)
+            throw error_t();
+        std::copy(first, last, data<value_t>());
+    }
+
+    message_t(const void *data_, size_t size_)
+    {
+        int rc = zmq_msg_init_size(&msg, size_);
+        if (rc != 0)
+            throw error_t();
+        if (size_) {
+            // this constructor allows (nullptr, 0),
+            // memcpy with a null pointer is UB
+            memcpy(data(), data_, size_);
+        }
+    }
+
+    message_t(void *data_, size_t size_, free_fn *ffn_, void *hint_ = ZMQ_NULLPTR)
+    {
+        int rc = zmq_msg_init_data(&msg, data_, size_, ffn_, hint_);
+        if (rc != 0)
+            throw error_t();
+    }
+
+    // overload set of string-like types and generic containers
+#if defined(ZMQ_CPP11) && !defined(ZMQ_CPP11_PARTIAL)
+    // NOTE this constructor will include the null terminator
+    // when called with a string literal.
+    // An overload taking const char* can not be added because
+    // it would be preferred over this function and break compatiblity.
+    template<
+      class Char,
+      size_t N,
+      typename = typename std::enable_if<detail::is_char_type<Char>::value>::type>
+    ZMQ_DEPRECATED("from 4.7.0, use constructors taking iterators, (pointer, size) "
+                   "or strings instead")
+    explicit message_t(const Char (&data)[N]) :
+        message_t(detail::ranges::begin(data), detail::ranges::end(data))
+    {
+    }
+
+    template<class Range,
+             typename = typename std::enable_if<
+               detail::is_range<Range>::value
+               && ZMQ_IS_TRIVIALLY_COPYABLE(detail::range_value_t<Range>)
+               && !detail::is_char_type<detail::range_value_t<Range>>::value
+               && !std::is_same<Range, message_t>::value>::type>
+    explicit message_t(const Range &rng) :
+        message_t(detail::ranges::begin(rng), detail::ranges::end(rng))
+    {
+    }
+
+    explicit message_t(const std::string &str) : message_t(str.data(), str.size()) {}
+
+#if CPPZMQ_HAS_STRING_VIEW
+    explicit message_t(std::string_view str) : message_t(str.data(), str.size()) {}
+#endif
+
+#endif
+
+#ifdef ZMQ_HAS_RVALUE_REFS
+    message_t(message_t &&rhs) ZMQ_NOTHROW : msg(rhs.msg)
+    {
+        int rc = zmq_msg_init(&rhs.msg);
+        ZMQ_ASSERT(rc == 0);
+    }
+
+    message_t &operator=(message_t &&rhs) ZMQ_NOTHROW
+    {
+        std::swap(msg, rhs.msg);
+        return *this;
+    }
+#endif
+
+    ~message_t() ZMQ_NOTHROW
+    {
+        int rc = zmq_msg_close(&msg);
+        ZMQ_ASSERT(rc == 0);
+    }
+
+    void rebuild()
+    {
+        int rc = zmq_msg_close(&msg);
+        if (rc != 0)
+            throw error_t();
+        rc = zmq_msg_init(&msg);
+        ZMQ_ASSERT(rc == 0);
+    }
+
+    void rebuild(size_t size_)
+    {
+        int rc = zmq_msg_close(&msg);
+        if (rc != 0)
+            throw error_t();
+        rc = zmq_msg_init_size(&msg, size_);
+        if (rc != 0)
+            throw error_t();
+    }
+
+    void rebuild(const void *data_, size_t size_)
+    {
+        int rc = zmq_msg_close(&msg);
+        if (rc != 0)
+            throw error_t();
+        rc = zmq_msg_init_size(&msg, size_);
+        if (rc != 0)
+            throw error_t();
+        memcpy(data(), data_, size_);
+    }
+
+    void rebuild(void *data_, size_t size_, free_fn *ffn_, void *hint_ = ZMQ_NULLPTR)
+    {
+        int rc = zmq_msg_close(&msg);
+        if (rc != 0)
+            throw error_t();
+        rc = zmq_msg_init_data(&msg, data_, size_, ffn_, hint_);
+        if (rc != 0)
+            throw error_t();
+    }
+
+    ZMQ_DEPRECATED("from 4.3.1, use move taking non-const reference instead")
+    void move(message_t const *msg_)
+    {
+        int rc = zmq_msg_move(&msg, const_cast<zmq_msg_t *>(msg_->handle()));
+        if (rc != 0)
+            throw error_t();
+    }
+
+    void move(message_t &msg_)
+    {
+        int rc = zmq_msg_move(&msg, msg_.handle());
+        if (rc != 0)
+            throw error_t();
+    }
+
+    ZMQ_DEPRECATED("from 4.3.1, use copy taking non-const reference instead")
+    void copy(message_t const *msg_)
+    {
+        int rc = zmq_msg_copy(&msg, const_cast<zmq_msg_t *>(msg_->handle()));
+        if (rc != 0)
+            throw error_t();
+    }
+
+    void copy(message_t &msg_)
+    {
+        int rc = zmq_msg_copy(&msg, msg_.handle());
+        if (rc != 0)
+            throw error_t();
+    }
+
+    bool more() const ZMQ_NOTHROW
+    {
+        int rc = zmq_msg_more(const_cast<zmq_msg_t *>(&msg));
+        return rc != 0;
+    }
+
+    void *data() ZMQ_NOTHROW { return zmq_msg_data(&msg); }
+
+    const void *data() const ZMQ_NOTHROW
+    {
+        return zmq_msg_data(const_cast<zmq_msg_t *>(&msg));
+    }
+
+    size_t size() const ZMQ_NOTHROW
+    {
+        return zmq_msg_size(const_cast<zmq_msg_t *>(&msg));
+    }
+
+    ZMQ_NODISCARD bool empty() const ZMQ_NOTHROW { return size() == 0u; }
+
+    template<typename T> T *data() ZMQ_NOTHROW { return static_cast<T *>(data()); }
+
+    template<typename T> T const *data() const ZMQ_NOTHROW
+    {
+        return static_cast<T const *>(data());
+    }
+
+    ZMQ_DEPRECATED("from 4.3.0, use operator== instead")
+    bool equal(const message_t *other) const ZMQ_NOTHROW { return *this == *other; }
+
+    bool operator==(const message_t &other) const ZMQ_NOTHROW
+    {
+        const size_t my_size = size();
+        return my_size == other.size() && 0 == memcmp(data(), other.data(), my_size);
+    }
+
+    bool operator!=(const message_t &other) const ZMQ_NOTHROW
+    {
+        return !(*this == other);
+    }
+
+#if ZMQ_VERSION >= ZMQ_MAKE_VERSION(3, 2, 0)
+    int get(int property_)
+    {
+        int value = zmq_msg_get(&msg, property_);
+        if (value == -1)
+            throw error_t();
+        return value;
+    }
+#endif
+
+#if ZMQ_VERSION >= ZMQ_MAKE_VERSION(4, 1, 0)
+    const char *gets(const char *property_)
+    {
+        const char *value = zmq_msg_gets(&msg, property_);
+        if (value == ZMQ_NULLPTR)
+            throw error_t();
+        return value;
+    }
+#endif
+
+#if defined(ZMQ_BUILD_DRAFT_API) && ZMQ_VERSION >= ZMQ_MAKE_VERSION(4, 2, 0)
+    uint32_t routing_id() const
+    {
+        return zmq_msg_routing_id(const_cast<zmq_msg_t *>(&msg));
+    }
+
+    void set_routing_id(uint32_t routing_id)
+    {
+        int rc = zmq_msg_set_routing_id(&msg, routing_id);
+        if (rc != 0)
+            throw error_t();
+    }
+
+    const char *group() const
+    {
+        return zmq_msg_group(const_cast<zmq_msg_t *>(&msg));
+    }
+
+    void set_group(const char *group)
+    {
+        int rc = zmq_msg_set_group(&msg, group);
+        if (rc != 0)
+            throw error_t();
+    }
+#endif
+
+    // interpret message content as a string
+    std::string to_string() const
+    {
+        return std::string(static_cast<const char *>(data()), size());
+    }
+#if CPPZMQ_HAS_STRING_VIEW
+    // interpret message content as a string
+    std::string_view to_string_view() const noexcept
+    {
+        return std::string_view(static_cast<const char *>(data()), size());
+    }
+#endif
+
+    /** Dump content to string for debugging.
+    *   Ascii chars are readable, the rest is printed as hex.
+    *   Probably ridiculously slow.
+    *   Use to_string() or to_string_view() for
+    *   interpreting the message as a string.
+    */
+    std::string str() const
+    {
+        // Partly mutuated from the same method in zmq::multipart_t
+        std::stringstream os;
+
+        const unsigned char *msg_data = this->data<unsigned char>();
+        unsigned char byte;
+        size_t size = this->size();
+        int is_ascii[2] = {0, 0};
+
+        os << "zmq::message_t [size " << std::dec << std::setw(3)
+           << std::setfill('0') << size << "] (";
+        // Totally arbitrary
+        if (size >= 1000) {
+            os << "... too big to print)";
+        } else {
+            while (size--) {
+                byte = *msg_data++;
+
+                is_ascii[1] = (byte >= 32 && byte < 127);
+                if (is_ascii[1] != is_ascii[0])
+                    os << " "; // Separate text/non text
+
+                if (is_ascii[1]) {
+                    os << byte;
+                } else {
+                    os << std::hex << std::uppercase << std::setw(2)
+                       << std::setfill('0') << static_cast<short>(byte);
+                }
+                is_ascii[0] = is_ascii[1];
+            }
+            os << ")";
+        }
+        return os.str();
+    }
+
+    void swap(message_t &other) ZMQ_NOTHROW
+    {
+        // this assumes zmq::msg_t from libzmq is trivially relocatable
+        std::swap(msg, other.msg);
+    }
+
+    ZMQ_NODISCARD zmq_msg_t *handle() ZMQ_NOTHROW { return &msg; }
+    ZMQ_NODISCARD const zmq_msg_t *handle() const ZMQ_NOTHROW { return &msg; }
+
+  private:
+    //  The underlying message
+    zmq_msg_t msg;
+
+    //  Disable implicit message copying, so that users won't use shared
+    //  messages (less efficient) without being aware of the fact.
+    message_t(const message_t &) ZMQ_DELETED_FUNCTION;
+    void operator=(const message_t &) ZMQ_DELETED_FUNCTION;
+};
+
+inline void swap(message_t &a, message_t &b) ZMQ_NOTHROW
+{
+    a.swap(b);
+}
+
+#ifdef ZMQ_CPP11
+enum class ctxopt
+{
+#ifdef ZMQ_BLOCKY
+    blocky = ZMQ_BLOCKY,
+#endif
+#ifdef ZMQ_IO_THREADS
+    io_threads = ZMQ_IO_THREADS,
+#endif
+#ifdef ZMQ_THREAD_SCHED_POLICY
+    thread_sched_policy = ZMQ_THREAD_SCHED_POLICY,
+#endif
+#ifdef ZMQ_THREAD_PRIORITY
+    thread_priority = ZMQ_THREAD_PRIORITY,
+#endif
+#ifdef ZMQ_THREAD_AFFINITY_CPU_ADD
+    thread_affinity_cpu_add = ZMQ_THREAD_AFFINITY_CPU_ADD,
+#endif
+#ifdef ZMQ_THREAD_AFFINITY_CPU_REMOVE
+    thread_affinity_cpu_remove = ZMQ_THREAD_AFFINITY_CPU_REMOVE,
+#endif
+#ifdef ZMQ_THREAD_NAME_PREFIX
+    thread_name_prefix = ZMQ_THREAD_NAME_PREFIX,
+#endif
+#ifdef ZMQ_MAX_MSGSZ
+    max_msgsz = ZMQ_MAX_MSGSZ,
+#endif
+#ifdef ZMQ_ZERO_COPY_RECV
+    zero_copy_recv = ZMQ_ZERO_COPY_RECV,
+#endif
+#ifdef ZMQ_MAX_SOCKETS
+    max_sockets = ZMQ_MAX_SOCKETS,
+#endif
+#ifdef ZMQ_SOCKET_LIMIT
+    socket_limit = ZMQ_SOCKET_LIMIT,
+#endif
+#ifdef ZMQ_IPV6
+    ipv6 = ZMQ_IPV6,
+#endif
+#ifdef ZMQ_MSG_T_SIZE
+    msg_t_size = ZMQ_MSG_T_SIZE
+#endif
+};
+#endif
+
+class context_t
+{
+  public:
+    context_t()
+    {
+        ptr = zmq_ctx_new();
+        if (ptr == ZMQ_NULLPTR)
+            throw error_t();
+    }
+
+
+    explicit context_t(int io_threads_, int max_sockets_ = ZMQ_MAX_SOCKETS_DFLT)
+    {
+        ptr = zmq_ctx_new();
+        if (ptr == ZMQ_NULLPTR)
+            throw error_t();
+
+        int rc = zmq_ctx_set(ptr, ZMQ_IO_THREADS, io_threads_);
+        ZMQ_ASSERT(rc == 0);
+
+        rc = zmq_ctx_set(ptr, ZMQ_MAX_SOCKETS, max_sockets_);
+        ZMQ_ASSERT(rc == 0);
+    }
+
+#ifdef ZMQ_HAS_RVALUE_REFS
+    context_t(context_t &&rhs) ZMQ_NOTHROW : ptr(rhs.ptr) { rhs.ptr = ZMQ_NULLPTR; }
+    context_t &operator=(context_t &&rhs) ZMQ_NOTHROW
+    {
+        close();
+        std::swap(ptr, rhs.ptr);
+        return *this;
+    }
+#endif
+
+    ~context_t() ZMQ_NOTHROW { close(); }
+
+    ZMQ_CPP11_DEPRECATED("from 4.7.0, use set taking zmq::ctxopt instead")
+    int setctxopt(int option_, int optval_)
+    {
+        int rc = zmq_ctx_set(ptr, option_, optval_);
+        ZMQ_ASSERT(rc == 0);
+        return rc;
+    }
+
+    ZMQ_CPP11_DEPRECATED("from 4.7.0, use get taking zmq::ctxopt instead")
+    int getctxopt(int option_) { return zmq_ctx_get(ptr, option_); }
+
+#ifdef ZMQ_CPP11
+    void set(ctxopt option, int optval)
+    {
+        int rc = zmq_ctx_set(ptr, static_cast<int>(option), optval);
+        if (rc == -1)
+            throw error_t();
+    }
+
+    ZMQ_NODISCARD int get(ctxopt option)
+    {
+        int rc = zmq_ctx_get(ptr, static_cast<int>(option));
+        // some options have a default value of -1
+        // which is unfortunate, and may result in errors
+        // that don't make sense
+        if (rc == -1)
+            throw error_t();
+        return rc;
+    }
+#endif
+
+    // Terminates context (see also shutdown()).
+    void close() ZMQ_NOTHROW
+    {
+        if (ptr == ZMQ_NULLPTR)
+            return;
+
+        int rc;
+        do {
+            rc = zmq_ctx_destroy(ptr);
+        } while (rc == -1 && errno == EINTR);
+
+        ZMQ_ASSERT(rc == 0);
+        ptr = ZMQ_NULLPTR;
+    }
+
+    // Shutdown context in preparation for termination (close()).
+    // Causes all blocking socket operations and any further
+    // socket operations to return with ETERM.
+    void shutdown() ZMQ_NOTHROW
+    {
+        if (ptr == ZMQ_NULLPTR)
+            return;
+        int rc = zmq_ctx_shutdown(ptr);
+        ZMQ_ASSERT(rc == 0);
+    }
+
+    //  Be careful with this, it's probably only useful for
+    //  using the C api together with an existing C++ api.
+    //  Normally you should never need to use this.
+    ZMQ_EXPLICIT operator void *() ZMQ_NOTHROW { return ptr; }
+
+    ZMQ_EXPLICIT operator void const *() const ZMQ_NOTHROW { return ptr; }
+
+    ZMQ_NODISCARD void *handle() ZMQ_NOTHROW { return ptr; }
+
+    ZMQ_DEPRECATED("from 4.7.0, use handle() != nullptr instead")
+    operator bool() const ZMQ_NOTHROW { return ptr != ZMQ_NULLPTR; }
+
+    void swap(context_t &other) ZMQ_NOTHROW { std::swap(ptr, other.ptr); }
+
+  private:
+    void *ptr;
+
+    context_t(const context_t &) ZMQ_DELETED_FUNCTION;
+    void operator=(const context_t &) ZMQ_DELETED_FUNCTION;
+};
+
+inline void swap(context_t &a, context_t &b) ZMQ_NOTHROW
+{
+    a.swap(b);
+}
+
+#ifdef ZMQ_CPP11
+
+struct recv_buffer_size
+{
+    size_t size;             // number of bytes written to buffer
+    size_t untruncated_size; // untruncated message size in bytes
+
+    ZMQ_NODISCARD bool truncated() const noexcept
+    {
+        return size != untruncated_size;
+    }
+};
+
+#if CPPZMQ_HAS_OPTIONAL
+
+using send_result_t = std::optional<size_t>;
+using recv_result_t = std::optional<size_t>;
+using recv_buffer_result_t = std::optional<recv_buffer_size>;
+
+#else
+
+namespace detail
+{
+// A C++11 type emulating the most basic
+// operations of std::optional for trivial types
+template<class T> class trivial_optional
+{
+  public:
+    static_assert(std::is_trivial<T>::value, "T must be trivial");
+    using value_type = T;
+
+    trivial_optional() = default;
+    trivial_optional(T value) noexcept : _value(value), _has_value(true) {}
+
+    const T *operator->() const noexcept
+    {
+        assert(_has_value);
+        return &_value;
+    }
+    T *operator->() noexcept
+    {
+        assert(_has_value);
+        return &_value;
+    }
+
+    const T &operator*() const noexcept
+    {
+        assert(_has_value);
+        return _value;
+    }
+    T &operator*() noexcept
+    {
+        assert(_has_value);
+        return _value;
+    }
+
+    T &value()
+    {
+        if (!_has_value)
+            throw std::exception();
+        return _value;
+    }
+    const T &value() const
+    {
+        if (!_has_value)
+            throw std::exception();
+        return _value;
+    }
+
+    explicit operator bool() const noexcept { return _has_value; }
+    bool has_value() const noexcept { return _has_value; }
+
+  private:
+    T _value{};
+    bool _has_value{false};
+};
+} // namespace detail
+
+using send_result_t = detail::trivial_optional<size_t>;
+using recv_result_t = detail::trivial_optional<size_t>;
+using recv_buffer_result_t = detail::trivial_optional<recv_buffer_size>;
+
+#endif
+
+namespace detail
+{
+template<class T> constexpr T enum_bit_or(T a, T b) noexcept
+{
+    static_assert(std::is_enum<T>::value, "must be enum");
+    using U = typename std::underlying_type<T>::type;
+    return static_cast<T>(static_cast<U>(a) | static_cast<U>(b));
+}
+template<class T> constexpr T enum_bit_and(T a, T b) noexcept
+{
+    static_assert(std::is_enum<T>::value, "must be enum");
+    using U = typename std::underlying_type<T>::type;
+    return static_cast<T>(static_cast<U>(a) & static_cast<U>(b));
+}
+template<class T> constexpr T enum_bit_xor(T a, T b) noexcept
+{
+    static_assert(std::is_enum<T>::value, "must be enum");
+    using U = typename std::underlying_type<T>::type;
+    return static_cast<T>(static_cast<U>(a) ^ static_cast<U>(b));
+}
+template<class T> constexpr T enum_bit_not(T a) noexcept
+{
+    static_assert(std::is_enum<T>::value, "must be enum");
+    using U = typename std::underlying_type<T>::type;
+    return static_cast<T>(~static_cast<U>(a));
+}
+} // namespace detail
+
+// partially satisfies named requirement BitmaskType
+enum class send_flags : int
+{
+    none = 0,
+    dontwait = ZMQ_DONTWAIT,
+    sndmore = ZMQ_SNDMORE
+};
+
+constexpr send_flags operator|(send_flags a, send_flags b) noexcept
+{
+    return detail::enum_bit_or(a, b);
+}
+constexpr send_flags operator&(send_flags a, send_flags b) noexcept
+{
+    return detail::enum_bit_and(a, b);
+}
+constexpr send_flags operator^(send_flags a, send_flags b) noexcept
+{
+    return detail::enum_bit_xor(a, b);
+}
+constexpr send_flags operator~(send_flags a) noexcept
+{
+    return detail::enum_bit_not(a);
+}
+
+// partially satisfies named requirement BitmaskType
+enum class recv_flags : int
+{
+    none = 0,
+    dontwait = ZMQ_DONTWAIT
+};
+
+constexpr recv_flags operator|(recv_flags a, recv_flags b) noexcept
+{
+    return detail::enum_bit_or(a, b);
+}
+constexpr recv_flags operator&(recv_flags a, recv_flags b) noexcept
+{
+    return detail::enum_bit_and(a, b);
+}
+constexpr recv_flags operator^(recv_flags a, recv_flags b) noexcept
+{
+    return detail::enum_bit_xor(a, b);
+}
+constexpr recv_flags operator~(recv_flags a) noexcept
+{
+    return detail::enum_bit_not(a);
+}
+
+
+// mutable_buffer, const_buffer and buffer are based on
+// the Networking TS specification, draft:
+// http://www.open-std.org/jtc1/sc22/wg21/docs/papers/2018/n4771.pdf
+
+class mutable_buffer
+{
+  public:
+    constexpr mutable_buffer() noexcept : _data(nullptr), _size(0) {}
+    constexpr mutable_buffer(void *p, size_t n) noexcept : _data(p), _size(n)
+    {
+#ifdef ZMQ_EXTENDED_CONSTEXPR
+        assert(p != nullptr || n == 0);
+#endif
+    }
+
+    constexpr void *data() const noexcept { return _data; }
+    constexpr size_t size() const noexcept { return _size; }
+    mutable_buffer &operator+=(size_t n) noexcept
+    {
+        // (std::min) is a workaround for when a min macro is defined
+        const auto shift = (std::min)(n, _size);
+        _data = static_cast<char *>(_data) + shift;
+        _size -= shift;
+        return *this;
+    }
+
+  private:
+    void *_data;
+    size_t _size;
+};
+
+inline mutable_buffer operator+(const mutable_buffer &mb, size_t n) noexcept
+{
+    return mutable_buffer(static_cast<char *>(mb.data()) + (std::min)(n, mb.size()),
+                          mb.size() - (std::min)(n, mb.size()));
+}
+inline mutable_buffer operator+(size_t n, const mutable_buffer &mb) noexcept
+{
+    return mb + n;
+}
+
+class const_buffer
+{
+  public:
+    constexpr const_buffer() noexcept : _data(nullptr), _size(0) {}
+    constexpr const_buffer(const void *p, size_t n) noexcept : _data(p), _size(n)
+    {
+#ifdef ZMQ_EXTENDED_CONSTEXPR
+        assert(p != nullptr || n == 0);
+#endif
+    }
+    constexpr const_buffer(const mutable_buffer &mb) noexcept :
+        _data(mb.data()),
+        _size(mb.size())
+    {
+    }
+
+    constexpr const void *data() const noexcept { return _data; }
+    constexpr size_t size() const noexcept { return _size; }
+    const_buffer &operator+=(size_t n) noexcept
+    {
+        const auto shift = (std::min)(n, _size);
+        _data = static_cast<const char *>(_data) + shift;
+        _size -= shift;
+        return *this;
+    }
+
+  private:
+    const void *_data;
+    size_t _size;
+};
+
+inline const_buffer operator+(const const_buffer &cb, size_t n) noexcept
+{
+    return const_buffer(static_cast<const char *>(cb.data())
+                          + (std::min)(n, cb.size()),
+                        cb.size() - (std::min)(n, cb.size()));
+}
+inline const_buffer operator+(size_t n, const const_buffer &cb) noexcept
+{
+    return cb + n;
+}
+
+// buffer creation
+
+constexpr mutable_buffer buffer(void *p, size_t n) noexcept
+{
+    return mutable_buffer(p, n);
+}
+constexpr const_buffer buffer(const void *p, size_t n) noexcept
+{
+    return const_buffer(p, n);
+}
+constexpr mutable_buffer buffer(const mutable_buffer &mb) noexcept
+{
+    return mb;
+}
+inline mutable_buffer buffer(const mutable_buffer &mb, size_t n) noexcept
+{
+    return mutable_buffer(mb.data(), (std::min)(mb.size(), n));
+}
+constexpr const_buffer buffer(const const_buffer &cb) noexcept
+{
+    return cb;
+}
+inline const_buffer buffer(const const_buffer &cb, size_t n) noexcept
+{
+    return const_buffer(cb.data(), (std::min)(cb.size(), n));
+}
+
+namespace detail
+{
+template<class T> struct is_buffer
+{
+    static constexpr bool value =
+      std::is_same<T, const_buffer>::value || std::is_same<T, mutable_buffer>::value;
+};
+
+template<class T> struct is_pod_like
+{
+    // NOTE: The networking draft N4771 section 16.11 requires
+    // T in the buffer functions below to be
+    // trivially copyable OR standard layout.
+    // Here we decide to be conservative and require both.
+    static constexpr bool value =
+      ZMQ_IS_TRIVIALLY_COPYABLE(T) && std::is_standard_layout<T>::value;
+};
+
+template<class C> constexpr auto seq_size(const C &c) noexcept -> decltype(c.size())
+{
+    return c.size();
+}
+template<class T, size_t N>
+constexpr size_t seq_size(const T (&/*array*/)[N]) noexcept
+{
+    return N;
+}
+
+template<class Seq>
+auto buffer_contiguous_sequence(Seq &&seq) noexcept
+  -> decltype(buffer(std::addressof(*std::begin(seq)), size_t{}))
+{
+    using T = typename std::remove_cv<
+      typename std::remove_reference<decltype(*std::begin(seq))>::type>::type;
+    static_assert(detail::is_pod_like<T>::value, "T must be POD");
+
+    const auto size = seq_size(seq);
+    return buffer(size != 0u ? std::addressof(*std::begin(seq)) : nullptr,
+                  size * sizeof(T));
+}
+template<class Seq>
+auto buffer_contiguous_sequence(Seq &&seq, size_t n_bytes) noexcept
+  -> decltype(buffer_contiguous_sequence(seq))
+{
+    using T = typename std::remove_cv<
+      typename std::remove_reference<decltype(*std::begin(seq))>::type>::type;
+    static_assert(detail::is_pod_like<T>::value, "T must be POD");
+
+    const auto size = seq_size(seq);
+    return buffer(size != 0u ? std::addressof(*std::begin(seq)) : nullptr,
+                  (std::min)(size * sizeof(T), n_bytes));
+}
+
+} // namespace detail
+
+// C array
+template<class T, size_t N> mutable_buffer buffer(T (&data)[N]) noexcept
+{
+    return detail::buffer_contiguous_sequence(data);
+}
+template<class T, size_t N>
+mutable_buffer buffer(T (&data)[N], size_t n_bytes) noexcept
+{
+    return detail::buffer_contiguous_sequence(data, n_bytes);
+}
+template<class T, size_t N> const_buffer buffer(const T (&data)[N]) noexcept
+{
+    return detail::buffer_contiguous_sequence(data);
+}
+template<class T, size_t N>
+const_buffer buffer(const T (&data)[N], size_t n_bytes) noexcept
+{
+    return detail::buffer_contiguous_sequence(data, n_bytes);
+}
+// std::array
+template<class T, size_t N> mutable_buffer buffer(std::array<T, N> &data) noexcept
+{
+    return detail::buffer_contiguous_sequence(data);
+}
+template<class T, size_t N>
+mutable_buffer buffer(std::array<T, N> &data, size_t n_bytes) noexcept
+{
+    return detail::buffer_contiguous_sequence(data, n_bytes);
+}
+template<class T, size_t N>
+const_buffer buffer(std::array<const T, N> &data) noexcept
+{
+    return detail::buffer_contiguous_sequence(data);
+}
+template<class T, size_t N>
+const_buffer buffer(std::array<const T, N> &data, size_t n_bytes) noexcept
+{
+    return detail::buffer_contiguous_sequence(data, n_bytes);
+}
+template<class T, size_t N>
+const_buffer buffer(const std::array<T, N> &data) noexcept
+{
+    return detail::buffer_contiguous_sequence(data);
+}
+template<class T, size_t N>
+const_buffer buffer(const std::array<T, N> &data, size_t n_bytes) noexcept
+{
+    return detail::buffer_contiguous_sequence(data, n_bytes);
+}
+// std::vector
+template<class T, class Allocator>
+mutable_buffer buffer(std::vector<T, Allocator> &data) noexcept
+{
+    return detail::buffer_contiguous_sequence(data);
+}
+template<class T, class Allocator>
+mutable_buffer buffer(std::vector<T, Allocator> &data, size_t n_bytes) noexcept
+{
+    return detail::buffer_contiguous_sequence(data, n_bytes);
+}
+template<class T, class Allocator>
+const_buffer buffer(const std::vector<T, Allocator> &data) noexcept
+{
+    return detail::buffer_contiguous_sequence(data);
+}
+template<class T, class Allocator>
+const_buffer buffer(const std::vector<T, Allocator> &data, size_t n_bytes) noexcept
+{
+    return detail::buffer_contiguous_sequence(data, n_bytes);
+}
+// std::basic_string
+template<class T, class Traits, class Allocator>
+mutable_buffer buffer(std::basic_string<T, Traits, Allocator> &data) noexcept
+{
+    return detail::buffer_contiguous_sequence(data);
+}
+template<class T, class Traits, class Allocator>
+mutable_buffer buffer(std::basic_string<T, Traits, Allocator> &data,
+                      size_t n_bytes) noexcept
+{
+    return detail::buffer_contiguous_sequence(data, n_bytes);
+}
+template<class T, class Traits, class Allocator>
+const_buffer buffer(const std::basic_string<T, Traits, Allocator> &data) noexcept
+{
+    return detail::buffer_contiguous_sequence(data);
+}
+template<class T, class Traits, class Allocator>
+const_buffer buffer(const std::basic_string<T, Traits, Allocator> &data,
+                    size_t n_bytes) noexcept
+{
+    return detail::buffer_contiguous_sequence(data, n_bytes);
+}
+
+#if CPPZMQ_HAS_STRING_VIEW
+// std::basic_string_view
+template<class T, class Traits>
+const_buffer buffer(std::basic_string_view<T, Traits> data) noexcept
+{
+    return detail::buffer_contiguous_sequence(data);
+}
+template<class T, class Traits>
+const_buffer buffer(std::basic_string_view<T, Traits> data, size_t n_bytes) noexcept
+{
+    return detail::buffer_contiguous_sequence(data, n_bytes);
+}
+#endif
+
+// Buffer for a string literal (null terminated)
+// where the buffer size excludes the terminating character.
+// Equivalent to zmq::buffer(std::string_view("...")).
+template<class Char, size_t N>
+constexpr const_buffer str_buffer(const Char (&data)[N]) noexcept
+{
+    static_assert(detail::is_pod_like<Char>::value, "Char must be POD");
+#ifdef ZMQ_EXTENDED_CONSTEXPR
+    assert(data[N - 1] == Char{0});
+#endif
+    return const_buffer(static_cast<const Char *>(data), (N - 1) * sizeof(Char));
+}
+
+namespace literals
+{
+constexpr const_buffer operator"" _zbuf(const char *str, size_t len) noexcept
+{
+    return const_buffer(str, len * sizeof(char));
+}
+constexpr const_buffer operator"" _zbuf(const wchar_t *str, size_t len) noexcept
+{
+    return const_buffer(str, len * sizeof(wchar_t));
+}
+constexpr const_buffer operator"" _zbuf(const char16_t *str, size_t len) noexcept
+{
+    return const_buffer(str, len * sizeof(char16_t));
+}
+constexpr const_buffer operator"" _zbuf(const char32_t *str, size_t len) noexcept
+{
+    return const_buffer(str, len * sizeof(char32_t));
+}
+}
+
+#endif // ZMQ_CPP11
+
+
+#ifdef ZMQ_CPP11
+namespace sockopt
+{
+// There are two types of options,
+// integral type with known compiler time size (int, bool, int64_t, uint64_t)
+// and arrays with dynamic size (strings, binary data).
+
+// BoolUnit: if true accepts values of type bool (but passed as T into libzmq)
+template<int Opt, class T, bool BoolUnit = false> struct integral_option
+{
+};
+
+// NullTerm:
+// 0: binary data
+// 1: null-terminated string (`getsockopt` size includes null)
+// 2: binary (size 32) or Z85 encoder string of size 41 (null included)
+template<int Opt, int NullTerm = 1> struct array_option
+{
+};
+
+#define ZMQ_DEFINE_INTEGRAL_OPT(OPT, NAME, TYPE)                                    \
+    using NAME##_t = integral_option<OPT, TYPE, false>;                             \
+    ZMQ_INLINE_VAR ZMQ_CONSTEXPR_VAR NAME##_t NAME {}
+#define ZMQ_DEFINE_INTEGRAL_BOOL_UNIT_OPT(OPT, NAME, TYPE)                          \
+    using NAME##_t = integral_option<OPT, TYPE, true>;                              \
+    ZMQ_INLINE_VAR ZMQ_CONSTEXPR_VAR NAME##_t NAME {}
+#define ZMQ_DEFINE_ARRAY_OPT(OPT, NAME)                                             \
+    using NAME##_t = array_option<OPT>;                                             \
+    ZMQ_INLINE_VAR ZMQ_CONSTEXPR_VAR NAME##_t NAME {}
+#define ZMQ_DEFINE_ARRAY_OPT_BINARY(OPT, NAME)                                      \
+    using NAME##_t = array_option<OPT, 0>;                                          \
+    ZMQ_INLINE_VAR ZMQ_CONSTEXPR_VAR NAME##_t NAME {}
+#define ZMQ_DEFINE_ARRAY_OPT_BIN_OR_Z85(OPT, NAME)                                  \
+    using NAME##_t = array_option<OPT, 2>;                                          \
+    ZMQ_INLINE_VAR ZMQ_CONSTEXPR_VAR NAME##_t NAME {}
+
+// duplicate definition from libzmq 4.3.3
+#if defined _WIN32
+#if defined _WIN64
+typedef unsigned __int64 cppzmq_fd_t;
+#else
+typedef unsigned int cppzmq_fd_t;
+#endif
+#else
+typedef int cppzmq_fd_t;
+#endif
+
+#ifdef ZMQ_AFFINITY
+ZMQ_DEFINE_INTEGRAL_OPT(ZMQ_AFFINITY, affinity, uint64_t);
+#endif
+#ifdef ZMQ_BACKLOG
+ZMQ_DEFINE_INTEGRAL_OPT(ZMQ_BACKLOG, backlog, int);
+#endif
+#ifdef ZMQ_BINDTODEVICE
+ZMQ_DEFINE_ARRAY_OPT_BINARY(ZMQ_BINDTODEVICE, bindtodevice);
+#endif
+#ifdef ZMQ_CONFLATE
+ZMQ_DEFINE_INTEGRAL_BOOL_UNIT_OPT(ZMQ_CONFLATE, conflate, int);
+#endif
+#ifdef ZMQ_CONNECT_ROUTING_ID
+ZMQ_DEFINE_ARRAY_OPT(ZMQ_CONNECT_ROUTING_ID, connect_routing_id);
+#endif
+#ifdef ZMQ_CONNECT_TIMEOUT
+ZMQ_DEFINE_INTEGRAL_OPT(ZMQ_CONNECT_TIMEOUT, connect_timeout, int);
+#endif
+#ifdef ZMQ_CURVE_PUBLICKEY
+ZMQ_DEFINE_ARRAY_OPT_BIN_OR_Z85(ZMQ_CURVE_PUBLICKEY, curve_publickey);
+#endif
+#ifdef ZMQ_CURVE_SECRETKEY
+ZMQ_DEFINE_ARRAY_OPT_BIN_OR_Z85(ZMQ_CURVE_SECRETKEY, curve_secretkey);
+#endif
+#ifdef ZMQ_CURVE_SERVER
+ZMQ_DEFINE_INTEGRAL_BOOL_UNIT_OPT(ZMQ_CURVE_SERVER, curve_server, int);
+#endif
+#ifdef ZMQ_CURVE_SERVERKEY
+ZMQ_DEFINE_ARRAY_OPT_BIN_OR_Z85(ZMQ_CURVE_SERVERKEY, curve_serverkey);
+#endif
+#ifdef ZMQ_EVENTS
+ZMQ_DEFINE_INTEGRAL_OPT(ZMQ_EVENTS, events, int);
+#endif
+#ifdef ZMQ_FD
+ZMQ_DEFINE_INTEGRAL_OPT(ZMQ_FD, fd, cppzmq_fd_t);
+#endif
+#ifdef ZMQ_GSSAPI_PLAINTEXT
+ZMQ_DEFINE_INTEGRAL_BOOL_UNIT_OPT(ZMQ_GSSAPI_PLAINTEXT, gssapi_plaintext, int);
+#endif
+#ifdef ZMQ_GSSAPI_SERVER
+ZMQ_DEFINE_INTEGRAL_BOOL_UNIT_OPT(ZMQ_GSSAPI_SERVER, gssapi_server, int);
+#endif
+#ifdef ZMQ_GSSAPI_SERVICE_PRINCIPAL
+ZMQ_DEFINE_ARRAY_OPT(ZMQ_GSSAPI_SERVICE_PRINCIPAL, gssapi_service_principal);
+#endif
+#ifdef ZMQ_GSSAPI_SERVICE_PRINCIPAL_NAMETYPE
+ZMQ_DEFINE_INTEGRAL_OPT(ZMQ_GSSAPI_SERVICE_PRINCIPAL_NAMETYPE,
+                        gssapi_service_principal_nametype,
+                        int);
+#endif
+#ifdef ZMQ_GSSAPI_PRINCIPAL
+ZMQ_DEFINE_ARRAY_OPT(ZMQ_GSSAPI_PRINCIPAL, gssapi_principal);
+#endif
+#ifdef ZMQ_GSSAPI_PRINCIPAL_NAMETYPE
+ZMQ_DEFINE_INTEGRAL_OPT(ZMQ_GSSAPI_PRINCIPAL_NAMETYPE,
+                        gssapi_principal_nametype,
+                        int);
+#endif
+#ifdef ZMQ_HANDSHAKE_IVL
+ZMQ_DEFINE_INTEGRAL_OPT(ZMQ_HANDSHAKE_IVL, handshake_ivl, int);
+#endif
+#ifdef ZMQ_HEARTBEAT_IVL
+ZMQ_DEFINE_INTEGRAL_OPT(ZMQ_HEARTBEAT_IVL, heartbeat_ivl, int);
+#endif
+#ifdef ZMQ_HEARTBEAT_TIMEOUT
+ZMQ_DEFINE_INTEGRAL_OPT(ZMQ_HEARTBEAT_TIMEOUT, heartbeat_timeout, int);
+#endif
+#ifdef ZMQ_HEARTBEAT_TTL
+ZMQ_DEFINE_INTEGRAL_OPT(ZMQ_HEARTBEAT_TTL, heartbeat_ttl, int);
+#endif
+#ifdef ZMQ_IMMEDIATE
+ZMQ_DEFINE_INTEGRAL_BOOL_UNIT_OPT(ZMQ_IMMEDIATE, immediate, int);
+#endif
+#ifdef ZMQ_INVERT_MATCHING
+ZMQ_DEFINE_INTEGRAL_BOOL_UNIT_OPT(ZMQ_INVERT_MATCHING, invert_matching, int);
+#endif
+#ifdef ZMQ_IPV6
+ZMQ_DEFINE_INTEGRAL_BOOL_UNIT_OPT(ZMQ_IPV6, ipv6, int);
+#endif
+#ifdef ZMQ_LAST_ENDPOINT
+ZMQ_DEFINE_ARRAY_OPT(ZMQ_LAST_ENDPOINT, last_endpoint);
+#endif
+#ifdef ZMQ_LINGER
+ZMQ_DEFINE_INTEGRAL_OPT(ZMQ_LINGER, linger, int);
+#endif
+#ifdef ZMQ_MAXMSGSIZE
+ZMQ_DEFINE_INTEGRAL_OPT(ZMQ_MAXMSGSIZE, maxmsgsize, int64_t);
+#endif
+#ifdef ZMQ_MECHANISM
+ZMQ_DEFINE_INTEGRAL_OPT(ZMQ_MECHANISM, mechanism, int);
+#endif
+#ifdef ZMQ_METADATA
+ZMQ_DEFINE_ARRAY_OPT(ZMQ_METADATA, metadata);
+#endif
+#ifdef ZMQ_MULTICAST_HOPS
+ZMQ_DEFINE_INTEGRAL_OPT(ZMQ_MULTICAST_HOPS, multicast_hops, int);
+#endif
+#ifdef ZMQ_MULTICAST_LOOP
+ZMQ_DEFINE_INTEGRAL_BOOL_UNIT_OPT(ZMQ_MULTICAST_LOOP, multicast_loop, int);
+#endif
+#ifdef ZMQ_MULTICAST_MAXTPDU
+ZMQ_DEFINE_INTEGRAL_OPT(ZMQ_MULTICAST_MAXTPDU, multicast_maxtpdu, int);
+#endif
+#ifdef ZMQ_PLAIN_SERVER
+ZMQ_DEFINE_INTEGRAL_BOOL_UNIT_OPT(ZMQ_PLAIN_SERVER, plain_server, int);
+#endif
+#ifdef ZMQ_PLAIN_PASSWORD
+ZMQ_DEFINE_ARRAY_OPT(ZMQ_PLAIN_PASSWORD, plain_password);
+#endif
+#ifdef ZMQ_PLAIN_USERNAME
+ZMQ_DEFINE_ARRAY_OPT(ZMQ_PLAIN_USERNAME, plain_username);
+#endif
+#ifdef ZMQ_USE_FD
+ZMQ_DEFINE_INTEGRAL_OPT(ZMQ_USE_FD, use_fd, int);
+#endif
+#ifdef ZMQ_PROBE_ROUTER
+ZMQ_DEFINE_INTEGRAL_BOOL_UNIT_OPT(ZMQ_PROBE_ROUTER, probe_router, int);
+#endif
+#ifdef ZMQ_RATE
+ZMQ_DEFINE_INTEGRAL_OPT(ZMQ_RATE, rate, int);
+#endif
+#ifdef ZMQ_RCVBUF
+ZMQ_DEFINE_INTEGRAL_OPT(ZMQ_RCVBUF, rcvbuf, int);
+#endif
+#ifdef ZMQ_RCVHWM
+ZMQ_DEFINE_INTEGRAL_OPT(ZMQ_RCVHWM, rcvhwm, int);
+#endif
+#ifdef ZMQ_RCVMORE
+ZMQ_DEFINE_INTEGRAL_BOOL_UNIT_OPT(ZMQ_RCVMORE, rcvmore, int);
+#endif
+#ifdef ZMQ_RCVTIMEO
+ZMQ_DEFINE_INTEGRAL_OPT(ZMQ_RCVTIMEO, rcvtimeo, int);
+#endif
+#ifdef ZMQ_RECONNECT_IVL
+ZMQ_DEFINE_INTEGRAL_OPT(ZMQ_RECONNECT_IVL, reconnect_ivl, int);
+#endif
+#ifdef ZMQ_RECONNECT_IVL_MAX
+ZMQ_DEFINE_INTEGRAL_OPT(ZMQ_RECONNECT_IVL_MAX, reconnect_ivl_max, int);
+#endif
+#ifdef ZMQ_RECOVERY_IVL
+ZMQ_DEFINE_INTEGRAL_OPT(ZMQ_RECOVERY_IVL, recovery_ivl, int);
+#endif
+#ifdef ZMQ_REQ_CORRELATE
+ZMQ_DEFINE_INTEGRAL_BOOL_UNIT_OPT(ZMQ_REQ_CORRELATE, req_correlate, int);
+#endif
+#ifdef ZMQ_REQ_RELAXED
+ZMQ_DEFINE_INTEGRAL_BOOL_UNIT_OPT(ZMQ_REQ_RELAXED, req_relaxed, int);
+#endif
+#ifdef ZMQ_ROUTER_HANDOVER
+ZMQ_DEFINE_INTEGRAL_BOOL_UNIT_OPT(ZMQ_ROUTER_HANDOVER, router_handover, int);
+#endif
+#ifdef ZMQ_ROUTER_MANDATORY
+ZMQ_DEFINE_INTEGRAL_BOOL_UNIT_OPT(ZMQ_ROUTER_MANDATORY, router_mandatory, int);
+#endif
+#ifdef ZMQ_ROUTER_NOTIFY
+ZMQ_DEFINE_INTEGRAL_OPT(ZMQ_ROUTER_NOTIFY, router_notify, int);
+#endif
+#ifdef ZMQ_ROUTING_ID
+ZMQ_DEFINE_ARRAY_OPT_BINARY(ZMQ_ROUTING_ID, routing_id);
+#endif
+#ifdef ZMQ_SNDBUF
+ZMQ_DEFINE_INTEGRAL_OPT(ZMQ_SNDBUF, sndbuf, int);
+#endif
+#ifdef ZMQ_SNDHWM
+ZMQ_DEFINE_INTEGRAL_OPT(ZMQ_SNDHWM, sndhwm, int);
+#endif
+#ifdef ZMQ_SNDTIMEO
+ZMQ_DEFINE_INTEGRAL_OPT(ZMQ_SNDTIMEO, sndtimeo, int);
+#endif
+#ifdef ZMQ_SOCKS_PROXY
+ZMQ_DEFINE_ARRAY_OPT(ZMQ_SOCKS_PROXY, socks_proxy);
+#endif
+#ifdef ZMQ_STREAM_NOTIFY
+ZMQ_DEFINE_INTEGRAL_BOOL_UNIT_OPT(ZMQ_STREAM_NOTIFY, stream_notify, int);
+#endif
+#ifdef ZMQ_SUBSCRIBE
+ZMQ_DEFINE_ARRAY_OPT(ZMQ_SUBSCRIBE, subscribe);
+#endif
+#ifdef ZMQ_TCP_KEEPALIVE
+ZMQ_DEFINE_INTEGRAL_OPT(ZMQ_TCP_KEEPALIVE, tcp_keepalive, int);
+#endif
+#ifdef ZMQ_TCP_KEEPALIVE_CNT
+ZMQ_DEFINE_INTEGRAL_OPT(ZMQ_TCP_KEEPALIVE_CNT, tcp_keepalive_cnt, int);
+#endif
+#ifdef ZMQ_TCP_KEEPALIVE_IDLE
+ZMQ_DEFINE_INTEGRAL_OPT(ZMQ_TCP_KEEPALIVE_IDLE, tcp_keepalive_idle, int);
+#endif
+#ifdef ZMQ_TCP_KEEPALIVE_INTVL
+ZMQ_DEFINE_INTEGRAL_OPT(ZMQ_TCP_KEEPALIVE_INTVL, tcp_keepalive_intvl, int);
+#endif
+#ifdef ZMQ_TCP_MAXRT
+ZMQ_DEFINE_INTEGRAL_OPT(ZMQ_TCP_MAXRT, tcp_maxrt, int);
+#endif
+#ifdef ZMQ_THREAD_SAFE
+ZMQ_DEFINE_INTEGRAL_BOOL_UNIT_OPT(ZMQ_THREAD_SAFE, thread_safe, int);
+#endif
+#ifdef ZMQ_TOS
+ZMQ_DEFINE_INTEGRAL_OPT(ZMQ_TOS, tos, int);
+#endif
+#ifdef ZMQ_TYPE
+ZMQ_DEFINE_INTEGRAL_OPT(ZMQ_TYPE, type, int);
+#endif
+#ifdef ZMQ_UNSUBSCRIBE
+ZMQ_DEFINE_ARRAY_OPT(ZMQ_UNSUBSCRIBE, unsubscribe);
+#endif
+#ifdef ZMQ_VMCI_BUFFER_SIZE
+ZMQ_DEFINE_INTEGRAL_OPT(ZMQ_VMCI_BUFFER_SIZE, vmci_buffer_size, uint64_t);
+#endif
+#ifdef ZMQ_VMCI_BUFFER_MIN_SIZE
+ZMQ_DEFINE_INTEGRAL_OPT(ZMQ_VMCI_BUFFER_MIN_SIZE, vmci_buffer_min_size, uint64_t);
+#endif
+#ifdef ZMQ_VMCI_BUFFER_MAX_SIZE
+ZMQ_DEFINE_INTEGRAL_OPT(ZMQ_VMCI_BUFFER_MAX_SIZE, vmci_buffer_max_size, uint64_t);
+#endif
+#ifdef ZMQ_VMCI_CONNECT_TIMEOUT
+ZMQ_DEFINE_INTEGRAL_OPT(ZMQ_VMCI_CONNECT_TIMEOUT, vmci_connect_timeout, int);
+#endif
+#ifdef ZMQ_XPUB_VERBOSE
+ZMQ_DEFINE_INTEGRAL_BOOL_UNIT_OPT(ZMQ_XPUB_VERBOSE, xpub_verbose, int);
+#endif
+#ifdef ZMQ_XPUB_VERBOSER
+ZMQ_DEFINE_INTEGRAL_BOOL_UNIT_OPT(ZMQ_XPUB_VERBOSER, xpub_verboser, int);
+#endif
+#ifdef ZMQ_XPUB_MANUAL
+ZMQ_DEFINE_INTEGRAL_BOOL_UNIT_OPT(ZMQ_XPUB_MANUAL, xpub_manual, int);
+#endif
+#ifdef ZMQ_XPUB_NODROP
+ZMQ_DEFINE_INTEGRAL_BOOL_UNIT_OPT(ZMQ_XPUB_NODROP, xpub_nodrop, int);
+#endif
+#ifdef ZMQ_XPUB_WELCOME_MSG
+ZMQ_DEFINE_ARRAY_OPT(ZMQ_XPUB_WELCOME_MSG, xpub_welcome_msg);
+#endif
+#ifdef ZMQ_ZAP_ENFORCE_DOMAIN
+ZMQ_DEFINE_INTEGRAL_BOOL_UNIT_OPT(ZMQ_ZAP_ENFORCE_DOMAIN, zap_enforce_domain, int);
+#endif
+#ifdef ZMQ_ZAP_DOMAIN
+ZMQ_DEFINE_ARRAY_OPT(ZMQ_ZAP_DOMAIN, zap_domain);
+#endif
+
+} // namespace sockopt
+#endif // ZMQ_CPP11
+
+
+namespace detail
+{
+class socket_base
+{
+  public:
+    socket_base() ZMQ_NOTHROW : _handle(ZMQ_NULLPTR) {}
+    ZMQ_EXPLICIT socket_base(void *handle) ZMQ_NOTHROW : _handle(handle) {}
+
+    template<typename T>
+    ZMQ_CPP11_DEPRECATED("from 4.7.0, use `set` taking option from zmq::sockopt")
+    void setsockopt(int option_, T const &optval)
+    {
+        setsockopt(option_, &optval, sizeof(T));
+    }
+
+    ZMQ_CPP11_DEPRECATED("from 4.7.0, use `set` taking option from zmq::sockopt")
+    void setsockopt(int option_, const void *optval_, size_t optvallen_)
+    {
+        int rc = zmq_setsockopt(_handle, option_, optval_, optvallen_);
+        if (rc != 0)
+            throw error_t();
+    }
+
+    ZMQ_CPP11_DEPRECATED("from 4.7.0, use `get` taking option from zmq::sockopt")
+    void getsockopt(int option_, void *optval_, size_t *optvallen_) const
+    {
+        int rc = zmq_getsockopt(_handle, option_, optval_, optvallen_);
+        if (rc != 0)
+            throw error_t();
+    }
+
+    template<typename T>
+    ZMQ_CPP11_DEPRECATED("from 4.7.0, use `get` taking option from zmq::sockopt")
+    T getsockopt(int option_) const
+    {
+        T optval;
+        size_t optlen = sizeof(T);
+        getsockopt(option_, &optval, &optlen);
+        return optval;
+    }
+
+#ifdef ZMQ_CPP11
+    // Set integral socket option, e.g.
+    // `socket.set(zmq::sockopt::linger, 0)`
+    template<int Opt, class T, bool BoolUnit>
+    void set(sockopt::integral_option<Opt, T, BoolUnit>, const T &val)
+    {
+        static_assert(std::is_integral<T>::value, "T must be integral");
+        set_option(Opt, &val, sizeof val);
+    }
+
+    // Set integral socket option from boolean, e.g.
+    // `socket.set(zmq::sockopt::immediate, false)`
+    template<int Opt, class T>
+    void set(sockopt::integral_option<Opt, T, true>, bool val)
+    {
+        static_assert(std::is_integral<T>::value, "T must be integral");
+        T rep_val = val;
+        set_option(Opt, &rep_val, sizeof rep_val);
+    }
+
+    // Set array socket option, e.g.
+    // `socket.set(zmq::sockopt::plain_username, "foo123")`
+    template<int Opt, int NullTerm>
+    void set(sockopt::array_option<Opt, NullTerm>, const char *buf)
+    {
+        set_option(Opt, buf, std::strlen(buf));
+    }
+
+    // Set array socket option, e.g.
+    // `socket.set(zmq::sockopt::routing_id, zmq::buffer(id))`
+    template<int Opt, int NullTerm>
+    void set(sockopt::array_option<Opt, NullTerm>, const_buffer buf)
+    {
+        set_option(Opt, buf.data(), buf.size());
+    }
+
+    // Set array socket option, e.g.
+    // `socket.set(zmq::sockopt::routing_id, id_str)`
+    template<int Opt, int NullTerm>
+    void set(sockopt::array_option<Opt, NullTerm>, const std::string &buf)
+    {
+        set_option(Opt, buf.data(), buf.size());
+    }
+
+#if CPPZMQ_HAS_STRING_VIEW
+    // Set array socket option, e.g.
+    // `socket.set(zmq::sockopt::routing_id, id_str)`
+    template<int Opt, int NullTerm>
+    void set(sockopt::array_option<Opt, NullTerm>, std::string_view buf)
+    {
+        set_option(Opt, buf.data(), buf.size());
+    }
+#endif
+
+    // Get scalar socket option, e.g.
+    // `auto opt = socket.get(zmq::sockopt::linger)`
+    template<int Opt, class T, bool BoolUnit>
+    ZMQ_NODISCARD T get(sockopt::integral_option<Opt, T, BoolUnit>) const
+    {
+        static_assert(std::is_integral<T>::value, "T must be integral");
+        T val;
+        size_t size = sizeof val;
+        get_option(Opt, &val, &size);
+        assert(size == sizeof val);
+        return val;
+    }
+
+    // Get array socket option, writes to buf, returns option size in bytes, e.g.
+    // `size_t optsize = socket.get(zmq::sockopt::routing_id, zmq::buffer(id))`
+    template<int Opt, int NullTerm>
+    ZMQ_NODISCARD size_t get(sockopt::array_option<Opt, NullTerm>,
+                             mutable_buffer buf) const
+    {
+        size_t size = buf.size();
+        get_option(Opt, buf.data(), &size);
+        return size;
+    }
+
+    // Get array socket option as string (initializes the string buffer size to init_size) e.g.
+    // `auto s = socket.get(zmq::sockopt::routing_id)`
+    // Note: removes the null character from null-terminated string options,
+    // i.e. the string size excludes the null character.
+    template<int Opt, int NullTerm>
+    ZMQ_NODISCARD std::string get(sockopt::array_option<Opt, NullTerm>,
+                                  size_t init_size = 1024) const
+    {
+        if (NullTerm == 2 && init_size == 1024) {
+            init_size = 41; // get as Z85 string
+        }
+        std::string str(init_size, '\0');
+        size_t size = get(sockopt::array_option<Opt>{}, buffer(str));
+        if (NullTerm == 1) {
+            if (size > 0) {
+                assert(str[size - 1] == '\0');
+                --size;
+            }
+        } else if (NullTerm == 2) {
+            assert(size == 32 || size == 41);
+            if (size == 41) {
+                assert(str[size - 1] == '\0');
+                --size;
+            }
+        }
+        str.resize(size);
+        return str;
+    }
+#endif
+
+    void bind(std::string const &addr) { bind(addr.c_str()); }
+
+    void bind(const char *addr_)
+    {
+        int rc = zmq_bind(_handle, addr_);
+        if (rc != 0)
+            throw error_t();
+    }
+
+    void unbind(std::string const &addr) { unbind(addr.c_str()); }
+
+    void unbind(const char *addr_)
+    {
+        int rc = zmq_unbind(_handle, addr_);
+        if (rc != 0)
+            throw error_t();
+    }
+
+    void connect(std::string const &addr) { connect(addr.c_str()); }
+
+    void connect(const char *addr_)
+    {
+        int rc = zmq_connect(_handle, addr_);
+        if (rc != 0)
+            throw error_t();
+    }
+
+    void disconnect(std::string const &addr) { disconnect(addr.c_str()); }
+
+    void disconnect(const char *addr_)
+    {
+        int rc = zmq_disconnect(_handle, addr_);
+        if (rc != 0)
+            throw error_t();
+    }
+
+    bool connected() const ZMQ_NOTHROW { return (_handle != ZMQ_NULLPTR); }
+
+    ZMQ_CPP11_DEPRECATED("from 4.3.1, use send taking a const_buffer and send_flags")
+    size_t send(const void *buf_, size_t len_, int flags_ = 0)
+    {
+        int nbytes = zmq_send(_handle, buf_, len_, flags_);
+        if (nbytes >= 0)
+            return static_cast<size_t>(nbytes);
+        if (zmq_errno() == EAGAIN)
+            return 0;
+        throw error_t();
+    }
+
+    ZMQ_CPP11_DEPRECATED("from 4.3.1, use send taking message_t and send_flags")
+    bool send(message_t &msg_,
+              int flags_ = 0) // default until removed
+    {
+        int nbytes = zmq_msg_send(msg_.handle(), _handle, flags_);
+        if (nbytes >= 0)
+            return true;
+        if (zmq_errno() == EAGAIN)
+            return false;
+        throw error_t();
+    }
+
+    template<typename T>
+    ZMQ_CPP11_DEPRECATED(
+      "from 4.4.1, use send taking message_t or buffer (for contiguous "
+      "ranges), and send_flags")
+    bool send(T first, T last, int flags_ = 0)
+    {
+        zmq::message_t msg(first, last);
+        int nbytes = zmq_msg_send(msg.handle(), _handle, flags_);
+        if (nbytes >= 0)
+            return true;
+        if (zmq_errno() == EAGAIN)
+            return false;
+        throw error_t();
+    }
+
+#ifdef ZMQ_HAS_RVALUE_REFS
+    ZMQ_CPP11_DEPRECATED("from 4.3.1, use send taking message_t and send_flags")
+    bool send(message_t &&msg_,
+              int flags_ = 0) // default until removed
+    {
+#ifdef ZMQ_CPP11
+        return send(msg_, static_cast<send_flags>(flags_)).has_value();
+#else
+        return send(msg_, flags_);
+#endif
+    }
+#endif
+
+#ifdef ZMQ_CPP11
+    send_result_t send(const_buffer buf, send_flags flags = send_flags::none)
+    {
+        const int nbytes =
+          zmq_send(_handle, buf.data(), buf.size(), static_cast<int>(flags));
+        if (nbytes >= 0)
+            return static_cast<size_t>(nbytes);
+        if (zmq_errno() == EAGAIN)
+            return {};
+        throw error_t();
+    }
+
+    send_result_t send(message_t &msg, send_flags flags)
+    {
+        int nbytes = zmq_msg_send(msg.handle(), _handle, static_cast<int>(flags));
+        if (nbytes >= 0)
+            return static_cast<size_t>(nbytes);
+        if (zmq_errno() == EAGAIN)
+            return {};
+        throw error_t();
+    }
+
+    send_result_t send(message_t &&msg, send_flags flags)
+    {
+        return send(msg, flags);
+    }
+#endif
+
+    ZMQ_CPP11_DEPRECATED(
+      "from 4.3.1, use recv taking a mutable_buffer and recv_flags")
+    size_t recv(void *buf_, size_t len_, int flags_ = 0)
+    {
+        int nbytes = zmq_recv(_handle, buf_, len_, flags_);
+        if (nbytes >= 0)
+            return static_cast<size_t>(nbytes);
+        if (zmq_errno() == EAGAIN)
+            return 0;
+        throw error_t();
+    }
+
+    ZMQ_CPP11_DEPRECATED(
+      "from 4.3.1, use recv taking a reference to message_t and recv_flags")
+    bool recv(message_t *msg_, int flags_ = 0)
+    {
+        int nbytes = zmq_msg_recv(msg_->handle(), _handle, flags_);
+        if (nbytes >= 0)
+            return true;
+        if (zmq_errno() == EAGAIN)
+            return false;
+        throw error_t();
+    }
+
+#ifdef ZMQ_CPP11
+    ZMQ_NODISCARD
+    recv_buffer_result_t recv(mutable_buffer buf,
+                              recv_flags flags = recv_flags::none)
+    {
+        const int nbytes =
+          zmq_recv(_handle, buf.data(), buf.size(), static_cast<int>(flags));
+        if (nbytes >= 0) {
+            return recv_buffer_size{
+              (std::min)(static_cast<size_t>(nbytes), buf.size()),
+              static_cast<size_t>(nbytes)};
+        }
+        if (zmq_errno() == EAGAIN)
+            return {};
+        throw error_t();
+    }
+
+    ZMQ_NODISCARD
+    recv_result_t recv(message_t &msg, recv_flags flags = recv_flags::none)
+    {
+        const int nbytes =
+          zmq_msg_recv(msg.handle(), _handle, static_cast<int>(flags));
+        if (nbytes >= 0) {
+            assert(msg.size() == static_cast<size_t>(nbytes));
+            return static_cast<size_t>(nbytes);
+        }
+        if (zmq_errno() == EAGAIN)
+            return {};
+        throw error_t();
+    }
+#endif
+
+#if defined(ZMQ_BUILD_DRAFT_API) && ZMQ_VERSION >= ZMQ_MAKE_VERSION(4, 2, 0)
+    void join(const char *group)
+    {
+        int rc = zmq_join(_handle, group);
+        if (rc != 0)
+            throw error_t();
+    }
+
+    void leave(const char *group)
+    {
+        int rc = zmq_leave(_handle, group);
+        if (rc != 0)
+            throw error_t();
+    }
+#endif
+
+    ZMQ_NODISCARD void *handle() ZMQ_NOTHROW { return _handle; }
+    ZMQ_NODISCARD const void *handle() const ZMQ_NOTHROW { return _handle; }
+
+    ZMQ_EXPLICIT operator bool() const ZMQ_NOTHROW { return _handle != ZMQ_NULLPTR; }
+    // note: non-const operator bool can be removed once
+    // operator void* is removed from socket_t
+    ZMQ_EXPLICIT operator bool() ZMQ_NOTHROW { return _handle != ZMQ_NULLPTR; }
+
+  protected:
+    void *_handle;
+
+  private:
+    void set_option(int option_, const void *optval_, size_t optvallen_)
+    {
+        int rc = zmq_setsockopt(_handle, option_, optval_, optvallen_);
+        if (rc != 0)
+            throw error_t();
+    }
+
+    void get_option(int option_, void *optval_, size_t *optvallen_) const
+    {
+        int rc = zmq_getsockopt(_handle, option_, optval_, optvallen_);
+        if (rc != 0)
+            throw error_t();
+    }
+};
+} // namespace detail
+
+#ifdef ZMQ_CPP11
+enum class socket_type : int
+{
+    req = ZMQ_REQ,
+    rep = ZMQ_REP,
+    dealer = ZMQ_DEALER,
+    router = ZMQ_ROUTER,
+    pub = ZMQ_PUB,
+    sub = ZMQ_SUB,
+    xpub = ZMQ_XPUB,
+    xsub = ZMQ_XSUB,
+    push = ZMQ_PUSH,
+    pull = ZMQ_PULL,
+#if defined(ZMQ_BUILD_DRAFT_API) && ZMQ_VERSION >= ZMQ_MAKE_VERSION(4, 2, 0)
+    server = ZMQ_SERVER,
+    client = ZMQ_CLIENT,
+    radio = ZMQ_RADIO,
+    dish = ZMQ_DISH,
+#endif
+#if ZMQ_VERSION_MAJOR >= 4
+    stream = ZMQ_STREAM,
+#endif
+    pair = ZMQ_PAIR
+};
+#endif
+
+struct from_handle_t
+{
+    struct _private
+    {
+    }; // disabling use other than with from_handle
+    ZMQ_CONSTEXPR_FN ZMQ_EXPLICIT from_handle_t(_private /*p*/) ZMQ_NOTHROW {}
+};
+
+ZMQ_CONSTEXPR_VAR from_handle_t from_handle =
+  from_handle_t(from_handle_t::_private());
+
+// A non-owning nullable reference to a socket.
+// The reference is invalidated on socket close or destruction.
+class socket_ref : public detail::socket_base
+{
+  public:
+    socket_ref() ZMQ_NOTHROW : detail::socket_base() {}
+#ifdef ZMQ_CPP11
+    socket_ref(std::nullptr_t) ZMQ_NOTHROW : detail::socket_base() {}
+#endif
+    socket_ref(from_handle_t /*fh*/, void *handle) ZMQ_NOTHROW
+        : detail::socket_base(handle)
+    {
+    }
+};
+
+#ifdef ZMQ_CPP11
+inline bool operator==(socket_ref sr, std::nullptr_t /*p*/) ZMQ_NOTHROW
+{
+    return sr.handle() == nullptr;
+}
+inline bool operator==(std::nullptr_t /*p*/, socket_ref sr) ZMQ_NOTHROW
+{
+    return sr.handle() == nullptr;
+}
+inline bool operator!=(socket_ref sr, std::nullptr_t /*p*/) ZMQ_NOTHROW
+{
+    return !(sr == nullptr);
+}
+inline bool operator!=(std::nullptr_t /*p*/, socket_ref sr) ZMQ_NOTHROW
+{
+    return !(sr == nullptr);
+}
+#endif
+
+inline bool operator==(socket_ref a, socket_ref b) ZMQ_NOTHROW
+{
+    return std::equal_to<void *>()(a.handle(), b.handle());
+}
+inline bool operator!=(socket_ref a, socket_ref b) ZMQ_NOTHROW
+{
+    return !(a == b);
+}
+inline bool operator<(socket_ref a, socket_ref b) ZMQ_NOTHROW
+{
+    return std::less<void *>()(a.handle(), b.handle());
+}
+inline bool operator>(socket_ref a, socket_ref b) ZMQ_NOTHROW
+{
+    return b < a;
+}
+inline bool operator<=(socket_ref a, socket_ref b) ZMQ_NOTHROW
+{
+    return !(a > b);
+}
+inline bool operator>=(socket_ref a, socket_ref b) ZMQ_NOTHROW
+{
+    return !(a < b);
+}
+
+} // namespace zmq
+
+#ifdef ZMQ_CPP11
+namespace std
+{
+template<> struct hash<zmq::socket_ref>
+{
+    size_t operator()(zmq::socket_ref sr) const ZMQ_NOTHROW
+    {
+        return hash<void *>()(sr.handle());
+    }
+};
+} // namespace std
+#endif
+
+namespace zmq
+{
+class socket_t : public detail::socket_base
+{
+    friend class monitor_t;
+
+  public:
+    socket_t() ZMQ_NOTHROW : detail::socket_base(ZMQ_NULLPTR), ctxptr(ZMQ_NULLPTR) {}
+
+    socket_t(context_t &context_, int type_) :
+        detail::socket_base(zmq_socket(context_.handle(), type_)),
+        ctxptr(context_.handle())
+    {
+        if (_handle == ZMQ_NULLPTR)
+            throw error_t();
+    }
+
+#ifdef ZMQ_CPP11
+    socket_t(context_t &context_, socket_type type_) :
+        socket_t(context_, static_cast<int>(type_))
+    {
+    }
+#endif
+
+#ifdef ZMQ_HAS_RVALUE_REFS
+    socket_t(socket_t &&rhs) ZMQ_NOTHROW : detail::socket_base(rhs._handle),
+                                           ctxptr(rhs.ctxptr)
+    {
+        rhs._handle = ZMQ_NULLPTR;
+        rhs.ctxptr = ZMQ_NULLPTR;
+    }
+    socket_t &operator=(socket_t &&rhs) ZMQ_NOTHROW
+    {
+        close();
+        std::swap(_handle, rhs._handle);
+        std::swap(ctxptr, rhs.ctxptr);
+        return *this;
+    }
+#endif
+
+    ~socket_t() ZMQ_NOTHROW { close(); }
+
+    operator void *() ZMQ_NOTHROW { return _handle; }
+
+    operator void const *() const ZMQ_NOTHROW { return _handle; }
+
+    void close() ZMQ_NOTHROW
+    {
+        if (_handle == ZMQ_NULLPTR)
+            // already closed
+            return;
+        int rc = zmq_close(_handle);
+        ZMQ_ASSERT(rc == 0);
+        _handle = ZMQ_NULLPTR;
+        ctxptr = ZMQ_NULLPTR;
+    }
+
+    void swap(socket_t &other) ZMQ_NOTHROW
+    {
+        std::swap(_handle, other._handle);
+        std::swap(ctxptr, other.ctxptr);
+    }
+
+    operator socket_ref() ZMQ_NOTHROW { return socket_ref(from_handle, _handle); }
+
+  private:
+    void *ctxptr;
+
+    socket_t(const socket_t &) ZMQ_DELETED_FUNCTION;
+    void operator=(const socket_t &) ZMQ_DELETED_FUNCTION;
+
+    // used by monitor_t
+    socket_t(void *context_, int type_) :
+        detail::socket_base(zmq_socket(context_, type_)),
+        ctxptr(context_)
+    {
+        if (_handle == ZMQ_NULLPTR)
+            throw error_t();
+        if (ctxptr == ZMQ_NULLPTR)
+            throw error_t();
+    }
+};
+
+inline void swap(socket_t &a, socket_t &b) ZMQ_NOTHROW
+{
+    a.swap(b);
+}
+
+ZMQ_DEPRECATED("from 4.3.1, use proxy taking socket_t objects")
+inline void proxy(void *frontend, void *backend, void *capture)
+{
+    int rc = zmq_proxy(frontend, backend, capture);
+    if (rc != 0)
+        throw error_t();
+}
+
+inline void
+proxy(socket_ref frontend, socket_ref backend, socket_ref capture = socket_ref())
+{
+    int rc = zmq_proxy(frontend.handle(), backend.handle(), capture.handle());
+    if (rc != 0)
+        throw error_t();
+}
+
+#ifdef ZMQ_HAS_PROXY_STEERABLE
+ZMQ_DEPRECATED("from 4.3.1, use proxy_steerable taking socket_t objects")
+inline void
+proxy_steerable(void *frontend, void *backend, void *capture, void *control)
+{
+    int rc = zmq_proxy_steerable(frontend, backend, capture, control);
+    if (rc != 0)
+        throw error_t();
+}
+
+inline void proxy_steerable(socket_ref frontend,
+                            socket_ref backend,
+                            socket_ref capture,
+                            socket_ref control)
+{
+    int rc = zmq_proxy_steerable(frontend.handle(), backend.handle(),
+                                 capture.handle(), control.handle());
+    if (rc != 0)
+        throw error_t();
+}
+#endif
+
+class monitor_t
+{
+  public:
+    monitor_t() : _socket(), _monitor_socket() {}
+
+    virtual ~monitor_t() { close(); }
+
+#ifdef ZMQ_HAS_RVALUE_REFS
+    monitor_t(monitor_t &&rhs) ZMQ_NOTHROW : _socket(), _monitor_socket()
+    {
+        std::swap(_socket, rhs._socket);
+        std::swap(_monitor_socket, rhs._monitor_socket);
+    }
+
+    monitor_t &operator=(monitor_t &&rhs) ZMQ_NOTHROW
+    {
+        close();
+        _socket = socket_ref();
+        std::swap(_socket, rhs._socket);
+        std::swap(_monitor_socket, rhs._monitor_socket);
+        return *this;
+    }
+#endif
+
+
+    void
+    monitor(socket_t &socket, std::string const &addr, int events = ZMQ_EVENT_ALL)
+    {
+        monitor(socket, addr.c_str(), events);
+    }
+
+    void monitor(socket_t &socket, const char *addr_, int events = ZMQ_EVENT_ALL)
+    {
+        init(socket, addr_, events);
+        while (true) {
+            check_event(-1);
+        }
+    }
+
+    void init(socket_t &socket, std::string const &addr, int events = ZMQ_EVENT_ALL)
+    {
+        init(socket, addr.c_str(), events);
+    }
+
+    void init(socket_t &socket, const char *addr_, int events = ZMQ_EVENT_ALL)
+    {
+        int rc = zmq_socket_monitor(socket.handle(), addr_, events);
+        if (rc != 0)
+            throw error_t();
+
+        _socket = socket;
+        _monitor_socket = socket_t(socket.ctxptr, ZMQ_PAIR);
+        _monitor_socket.connect(addr_);
+
+        on_monitor_started();
+    }
+
+    bool check_event(int timeout = 0)
+    {
+        assert(_monitor_socket);
+
+        zmq_msg_t eventMsg;
+        zmq_msg_init(&eventMsg);
+
+        zmq::pollitem_t items[] = {
+          {_monitor_socket.handle(), 0, ZMQ_POLLIN, 0},
+        };
+
+        zmq::poll(&items[0], 1, timeout);
+
+        if (items[0].revents & ZMQ_POLLIN) {
+            int rc = zmq_msg_recv(&eventMsg, _monitor_socket.handle(), 0);
+            if (rc == -1 && zmq_errno() == ETERM)
+                return false;
+            assert(rc != -1);
+
+        } else {
+            zmq_msg_close(&eventMsg);
+            return false;
+        }
+
+#if ZMQ_VERSION_MAJOR >= 4
+        const char *data = static_cast<const char *>(zmq_msg_data(&eventMsg));
+        zmq_event_t msgEvent;
+        memcpy(&msgEvent.event, data, sizeof(uint16_t));
+        data += sizeof(uint16_t);
+        memcpy(&msgEvent.value, data, sizeof(int32_t));
+        zmq_event_t *event = &msgEvent;
+#else
+        zmq_event_t *event = static_cast<zmq_event_t *>(zmq_msg_data(&eventMsg));
+#endif
+
+#ifdef ZMQ_NEW_MONITOR_EVENT_LAYOUT
+        zmq_msg_t addrMsg;
+        zmq_msg_init(&addrMsg);
+        int rc = zmq_msg_recv(&addrMsg, _monitor_socket.handle(), 0);
+        if (rc == -1 && zmq_errno() == ETERM) {
+            zmq_msg_close(&eventMsg);
+            return false;
+        }
+
+        assert(rc != -1);
+        const char *str = static_cast<const char *>(zmq_msg_data(&addrMsg));
+        std::string address(str, str + zmq_msg_size(&addrMsg));
+        zmq_msg_close(&addrMsg);
+#else
+        // Bit of a hack, but all events in the zmq_event_t union have the same layout so this will work for all event types.
+        std::string address = event->data.connected.addr;
+#endif
+
+#ifdef ZMQ_EVENT_MONITOR_STOPPED
+        if (event->event == ZMQ_EVENT_MONITOR_STOPPED) {
+            zmq_msg_close(&eventMsg);
+            return false;
+        }
+
+#endif
+
+        switch (event->event) {
+            case ZMQ_EVENT_CONNECTED:
+                on_event_connected(*event, address.c_str());
+                break;
+            case ZMQ_EVENT_CONNECT_DELAYED:
+                on_event_connect_delayed(*event, address.c_str());
+                break;
+            case ZMQ_EVENT_CONNECT_RETRIED:
+                on_event_connect_retried(*event, address.c_str());
+                break;
+            case ZMQ_EVENT_LISTENING:
+                on_event_listening(*event, address.c_str());
+                break;
+            case ZMQ_EVENT_BIND_FAILED:
+                on_event_bind_failed(*event, address.c_str());
+                break;
+            case ZMQ_EVENT_ACCEPTED:
+                on_event_accepted(*event, address.c_str());
+                break;
+            case ZMQ_EVENT_ACCEPT_FAILED:
+                on_event_accept_failed(*event, address.c_str());
+                break;
+            case ZMQ_EVENT_CLOSED:
+                on_event_closed(*event, address.c_str());
+                break;
+            case ZMQ_EVENT_CLOSE_FAILED:
+                on_event_close_failed(*event, address.c_str());
+                break;
+            case ZMQ_EVENT_DISCONNECTED:
+                on_event_disconnected(*event, address.c_str());
+                break;
+#ifdef ZMQ_BUILD_DRAFT_API
+#if ZMQ_VERSION >= ZMQ_MAKE_VERSION(4, 2, 3)
+            case ZMQ_EVENT_HANDSHAKE_FAILED_NO_DETAIL:
+                on_event_handshake_failed_no_detail(*event, address.c_str());
+                break;
+            case ZMQ_EVENT_HANDSHAKE_FAILED_PROTOCOL:
+                on_event_handshake_failed_protocol(*event, address.c_str());
+                break;
+            case ZMQ_EVENT_HANDSHAKE_FAILED_AUTH:
+                on_event_handshake_failed_auth(*event, address.c_str());
+                break;
+            case ZMQ_EVENT_HANDSHAKE_SUCCEEDED:
+                on_event_handshake_succeeded(*event, address.c_str());
+                break;
+#elif ZMQ_VERSION >= ZMQ_MAKE_VERSION(4, 2, 1)
+            case ZMQ_EVENT_HANDSHAKE_FAILED:
+                on_event_handshake_failed(*event, address.c_str());
+                break;
+            case ZMQ_EVENT_HANDSHAKE_SUCCEED:
+                on_event_handshake_succeed(*event, address.c_str());
+                break;
+#endif
+#endif
+            default:
+                on_event_unknown(*event, address.c_str());
+                break;
+        }
+        zmq_msg_close(&eventMsg);
+
+        return true;
+    }
+
+#ifdef ZMQ_EVENT_MONITOR_STOPPED
+    void abort()
+    {
+        if (_socket)
+            zmq_socket_monitor(_socket.handle(), ZMQ_NULLPTR, 0);
+
+        _socket = socket_ref();
+    }
+#endif
+    virtual void on_monitor_started() {}
+    virtual void on_event_connected(const zmq_event_t &event_, const char *addr_)
+    {
+        (void) event_;
+        (void) addr_;
+    }
+    virtual void on_event_connect_delayed(const zmq_event_t &event_,
+                                          const char *addr_)
+    {
+        (void) event_;
+        (void) addr_;
+    }
+    virtual void on_event_connect_retried(const zmq_event_t &event_,
+                                          const char *addr_)
+    {
+        (void) event_;
+        (void) addr_;
+    }
+    virtual void on_event_listening(const zmq_event_t &event_, const char *addr_)
+    {
+        (void) event_;
+        (void) addr_;
+    }
+    virtual void on_event_bind_failed(const zmq_event_t &event_, const char *addr_)
+    {
+        (void) event_;
+        (void) addr_;
+    }
+    virtual void on_event_accepted(const zmq_event_t &event_, const char *addr_)
+    {
+        (void) event_;
+        (void) addr_;
+    }
+    virtual void on_event_accept_failed(const zmq_event_t &event_, const char *addr_)
+    {
+        (void) event_;
+        (void) addr_;
+    }
+    virtual void on_event_closed(const zmq_event_t &event_, const char *addr_)
+    {
+        (void) event_;
+        (void) addr_;
+    }
+    virtual void on_event_close_failed(const zmq_event_t &event_, const char *addr_)
+    {
+        (void) event_;
+        (void) addr_;
+    }
+    virtual void on_event_disconnected(const zmq_event_t &event_, const char *addr_)
+    {
+        (void) event_;
+        (void) addr_;
+    }
+#if ZMQ_VERSION >= ZMQ_MAKE_VERSION(4, 2, 3)
+    virtual void on_event_handshake_failed_no_detail(const zmq_event_t &event_,
+                                                     const char *addr_)
+    {
+        (void) event_;
+        (void) addr_;
+    }
+    virtual void on_event_handshake_failed_protocol(const zmq_event_t &event_,
+                                                    const char *addr_)
+    {
+        (void) event_;
+        (void) addr_;
+    }
+    virtual void on_event_handshake_failed_auth(const zmq_event_t &event_,
+                                                const char *addr_)
+    {
+        (void) event_;
+        (void) addr_;
+    }
+    virtual void on_event_handshake_succeeded(const zmq_event_t &event_,
+                                              const char *addr_)
+    {
+        (void) event_;
+        (void) addr_;
+    }
+#elif ZMQ_VERSION >= ZMQ_MAKE_VERSION(4, 2, 1)
+    virtual void on_event_handshake_failed(const zmq_event_t &event_,
+                                           const char *addr_)
+    {
+        (void) event_;
+        (void) addr_;
+    }
+    virtual void on_event_handshake_succeed(const zmq_event_t &event_,
+                                            const char *addr_)
+    {
+        (void) event_;
+        (void) addr_;
+    }
+#endif
+    virtual void on_event_unknown(const zmq_event_t &event_, const char *addr_)
+    {
+        (void) event_;
+        (void) addr_;
+    }
+
+  private:
+    monitor_t(const monitor_t &) ZMQ_DELETED_FUNCTION;
+    void operator=(const monitor_t &) ZMQ_DELETED_FUNCTION;
+
+    socket_ref _socket;
+    socket_t _monitor_socket;
+
+    void close() ZMQ_NOTHROW
+    {
+        if (_socket)
+            zmq_socket_monitor(_socket.handle(), ZMQ_NULLPTR, 0);
+        _monitor_socket.close();
+    }
+};
+
+#if defined(ZMQ_BUILD_DRAFT_API) && defined(ZMQ_CPP11) && defined(ZMQ_HAVE_POLLER)
+
+// polling events
+enum class event_flags : short
+{
+    none = 0,
+    pollin = ZMQ_POLLIN,
+    pollout = ZMQ_POLLOUT,
+    pollerr = ZMQ_POLLERR,
+    pollpri = ZMQ_POLLPRI
+};
+
+constexpr event_flags operator|(event_flags a, event_flags b) noexcept
+{
+    return detail::enum_bit_or(a, b);
+}
+constexpr event_flags operator&(event_flags a, event_flags b) noexcept
+{
+    return detail::enum_bit_and(a, b);
+}
+constexpr event_flags operator^(event_flags a, event_flags b) noexcept
+{
+    return detail::enum_bit_xor(a, b);
+}
+constexpr event_flags operator~(event_flags a) noexcept
+{
+    return detail::enum_bit_not(a);
+}
+
+struct no_user_data;
+
+// layout compatible with zmq_poller_event_t
+template<class T = no_user_data> struct poller_event
+{
+    socket_ref socket;
+#ifdef _WIN32
+    SOCKET fd;
+#else
+    int fd;
+#endif
+    T *user_data;
+    event_flags events;
+};
+
+template<typename T = no_user_data> class poller_t
+{
+  public:
+    using event_type = poller_event<T>;
+
+    poller_t() : poller_ptr(zmq_poller_new())
+    {
+        if (!poller_ptr)
+            throw error_t();
+    }
+
+    template<
+      typename Dummy = void,
+      typename =
+        typename std::enable_if<!std::is_same<T, no_user_data>::value, Dummy>::type>
+    void add(zmq::socket_ref socket, event_flags events, T *user_data)
+    {
+        add_impl(socket, events, user_data);
+    }
+
+    void add(zmq::socket_ref socket, event_flags events)
+    {
+        add_impl(socket, events, nullptr);
+    }
+
+    void remove(zmq::socket_ref socket)
+    {
+        if (0 != zmq_poller_remove(poller_ptr.get(), socket.handle())) {
+            throw error_t();
+        }
+    }
+
+    void modify(zmq::socket_ref socket, event_flags events)
+    {
+        if (0
+            != zmq_poller_modify(poller_ptr.get(), socket.handle(),
+                                 static_cast<short>(events))) {
+            throw error_t();
+        }
+    }
+
+    size_t wait_all(std::vector<event_type> &poller_events,
+                    const std::chrono::milliseconds timeout)
+    {
+        int rc = zmq_poller_wait_all(
+          poller_ptr.get(),
+          reinterpret_cast<zmq_poller_event_t *>(poller_events.data()),
+          static_cast<int>(poller_events.size()),
+          static_cast<long>(timeout.count()));
+        if (rc > 0)
+            return static_cast<size_t>(rc);
+
+#if ZMQ_VERSION >= ZMQ_MAKE_VERSION(4, 2, 3)
+        if (zmq_errno() == EAGAIN)
+#else
+        if (zmq_errno() == ETIMEDOUT)
+#endif
+            return 0;
+
+        throw error_t();
+    }
+
+  private:
+    struct destroy_poller_t
+    {
+        void operator()(void *ptr) noexcept
+        {
+            int rc = zmq_poller_destroy(&ptr);
+            ZMQ_ASSERT(rc == 0);
+        }
+    };
+
+    std::unique_ptr<void, destroy_poller_t> poller_ptr;
+
+    void add_impl(zmq::socket_ref socket, event_flags events, T *user_data)
+    {
+        if (0
+            != zmq_poller_add(poller_ptr.get(), socket.handle(), user_data,
+                              static_cast<short>(events))) {
+            throw error_t();
+        }
+    }
+};
+#endif //  defined(ZMQ_BUILD_DRAFT_API) && defined(ZMQ_CPP11) && defined(ZMQ_HAVE_POLLER)
+
+inline std::ostream &operator<<(std::ostream &os, const message_t &msg)
+{
+    return os << msg.str();
+}
+
+} // namespace zmq
+
+#endif // __ZMQ_HPP_INCLUDED__
diff --git a/src/private/tinyxml2.cpp b/src/private/tinyxml2.cpp
new file mode 100755 (executable)
index 0000000..41b1f39
--- /dev/null
@@ -0,0 +1,2837 @@
+/*\r
+Original code by Lee Thomason (www.grinninglizard.com)\r
+\r
+This software is provided 'as-is', without any express or implied\r
+warranty. In no event will the authors be held liable for any\r
+damages arising from the use of this software.\r
+\r
+Permission is granted to anyone to use this software for any\r
+purpose, including commercial applications, and to alter it and\r
+redistribute it freely, subject to the following restrictions:\r
+\r
+1. The origin of this software must not be misrepresented; you must\r
+not claim that you wrote the original software. If you use this\r
+software in a product, an acknowledgment in the product documentation\r
+would be appreciated but is not required.\r
+\r
+2. Altered source versions must be plainly marked as such, and\r
+must not be misrepresented as being the original software.\r
+\r
+3. This notice may not be removed or altered from any source\r
+distribution.\r
+*/\r
+\r
+#include "tinyxml2.h"\r
+\r
+#include <new>         // yes, this one new style header, is in the Android SDK.\r
+#if defined(ANDROID_NDK) || defined(__BORLANDC__) || defined(__QNXNTO__)\r
+#   include <stddef.h>\r
+#   include <stdarg.h>\r
+#else\r
+#   include <cstddef>\r
+#   include <cstdarg>\r
+#endif\r
+\r
+#if defined(_MSC_VER) && (_MSC_VER >= 1400 ) && (!defined WINCE)\r
+       // Microsoft Visual Studio, version 2005 and higher. Not WinCE.\r
+       /*int _snprintf_s(\r
+          char *buffer,\r
+          size_t sizeOfBuffer,\r
+          size_t count,\r
+          const char *format [,\r
+                 argument] ...\r
+       );*/\r
+       static inline int TIXML_SNPRINTF( char* buffer, size_t size, const char* format, ... )\r
+       {\r
+               va_list va;\r
+               va_start( va, format );\r
+               int result = vsnprintf_s( buffer, size, _TRUNCATE, format, va );\r
+               va_end( va );\r
+               return result;\r
+       }\r
+\r
+       static inline int TIXML_VSNPRINTF( char* buffer, size_t size, const char* format, va_list va )\r
+       {\r
+               int result = vsnprintf_s( buffer, size, _TRUNCATE, format, va );\r
+               return result;\r
+       }\r
+\r
+       #define TIXML_VSCPRINTF _vscprintf\r
+       #define TIXML_SSCANF    sscanf_s\r
+#elif defined _MSC_VER\r
+       // Microsoft Visual Studio 2003 and earlier or WinCE\r
+       #define TIXML_SNPRINTF  _snprintf\r
+       #define TIXML_VSNPRINTF _vsnprintf\r
+       #define TIXML_SSCANF    sscanf\r
+       #if (_MSC_VER < 1400 ) && (!defined WINCE)\r
+               // Microsoft Visual Studio 2003 and not WinCE.\r
+               #define TIXML_VSCPRINTF   _vscprintf // VS2003's C runtime has this, but VC6 C runtime or WinCE SDK doesn't have.\r
+       #else\r
+               // Microsoft Visual Studio 2003 and earlier or WinCE.\r
+               static inline int TIXML_VSCPRINTF( const char* format, va_list va )\r
+               {\r
+                       int len = 512;\r
+                       for (;;) {\r
+                               len = len*2;\r
+                               char* str = new char[len]();\r
+                               const int required = _vsnprintf(str, len, format, va);\r
+                               delete[] str;\r
+                               if ( required != -1 ) {\r
+                                       TIXMLASSERT( required >= 0 );\r
+                                       len = required;\r
+                                       break;\r
+                               }\r
+                       }\r
+                       TIXMLASSERT( len >= 0 );\r
+                       return len;\r
+               }\r
+       #endif\r
+#else\r
+       // GCC version 3 and higher\r
+       //#warning( "Using sn* functions." )\r
+       #define TIXML_SNPRINTF  snprintf\r
+       #define TIXML_VSNPRINTF vsnprintf\r
+       static inline int TIXML_VSCPRINTF( const char* format, va_list va )\r
+       {\r
+               int len = vsnprintf( 0, 0, format, va );\r
+               TIXMLASSERT( len >= 0 );\r
+               return len;\r
+       }\r
+       #define TIXML_SSCANF   sscanf\r
+#endif\r
+\r
+\r
+static const char LINE_FEED                            = (char)0x0a;                   // all line endings are normalized to LF\r
+static const char LF = LINE_FEED;\r
+static const char CARRIAGE_RETURN              = (char)0x0d;                   // CR gets filtered out\r
+static const char CR = CARRIAGE_RETURN;\r
+static const char SINGLE_QUOTE                 = '\'';\r
+static const char DOUBLE_QUOTE                 = '\"';\r
+\r
+// Bunch of unicode info at:\r
+//             http://www.unicode.org/faq/utf_bom.html\r
+//     ef bb bf (Microsoft "lead bytes") - designates UTF-8\r
+\r
+static const unsigned char TIXML_UTF_LEAD_0 = 0xefU;\r
+static const unsigned char TIXML_UTF_LEAD_1 = 0xbbU;\r
+static const unsigned char TIXML_UTF_LEAD_2 = 0xbfU;\r
+\r
+namespace BT_TinyXML2\r
+{\r
+\r
+struct Entity {\r
+    const char* pattern;\r
+    int length;\r
+    char value;\r
+};\r
+\r
+static const int NUM_ENTITIES = 5;\r
+static const Entity entities[NUM_ENTITIES] = {\r
+    { "quot", 4,       DOUBLE_QUOTE },\r
+    { "amp", 3,                '&'  },\r
+    { "apos", 4,       SINGLE_QUOTE },\r
+    { "lt",    2,              '<'      },\r
+    { "gt",    2,              '>'      }\r
+};\r
+\r
+\r
+StrPair::~StrPair()\r
+{\r
+    Reset();\r
+}\r
+\r
+\r
+void StrPair::TransferTo( StrPair* other )\r
+{\r
+    if ( this == other ) {\r
+        return;\r
+    }\r
+    // This in effect implements the assignment operator by "moving"\r
+    // ownership (as in auto_ptr).\r
+\r
+    TIXMLASSERT( other != 0 );\r
+    TIXMLASSERT( other->_flags == 0 );\r
+    TIXMLASSERT( other->_start == 0 );\r
+    TIXMLASSERT( other->_end == 0 );\r
+\r
+    other->Reset();\r
+\r
+    other->_flags = _flags;\r
+    other->_start = _start;\r
+    other->_end = _end;\r
+\r
+    _flags = 0;\r
+    _start = 0;\r
+    _end = 0;\r
+}\r
+\r
+\r
+void StrPair::Reset()\r
+{\r
+    if ( _flags & NEEDS_DELETE ) {\r
+        delete [] _start;\r
+    }\r
+    _flags = 0;\r
+    _start = 0;\r
+    _end = 0;\r
+}\r
+\r
+\r
+void StrPair::SetStr( const char* str, int flags )\r
+{\r
+    TIXMLASSERT( str );\r
+    Reset();\r
+    size_t len = strlen( str );\r
+    TIXMLASSERT( _start == 0 );\r
+    _start = new char[ len+1 ];\r
+    memcpy( _start, str, len+1 );\r
+    _end = _start + len;\r
+    _flags = flags | NEEDS_DELETE;\r
+}\r
+\r
+\r
+char* StrPair::ParseText( char* p, const char* endTag, int strFlags, int* curLineNumPtr )\r
+{\r
+    TIXMLASSERT( p );\r
+    TIXMLASSERT( endTag && *endTag );\r
+       TIXMLASSERT(curLineNumPtr);\r
+\r
+    char* start = p;\r
+    char  endChar = *endTag;\r
+    size_t length = strlen( endTag );\r
+\r
+    // Inner loop of text parsing.\r
+    while ( *p ) {\r
+        if ( *p == endChar && strncmp( p, endTag, length ) == 0 ) {\r
+            Set( start, p, strFlags );\r
+            return p + length;\r
+        } else if (*p == '\n') {\r
+            ++(*curLineNumPtr);\r
+        }\r
+        ++p;\r
+        TIXMLASSERT( p );\r
+    }\r
+    return 0;\r
+}\r
+\r
+\r
+char* StrPair::ParseName( char* p )\r
+{\r
+    if ( !p || !(*p) ) {\r
+        return 0;\r
+    }\r
+    if ( !XMLUtil::IsNameStartChar( *p ) ) {\r
+        return 0;\r
+    }\r
+\r
+    char* const start = p;\r
+    ++p;\r
+    while ( *p && XMLUtil::IsNameChar( *p ) ) {\r
+        ++p;\r
+    }\r
+\r
+    Set( start, p, 0 );\r
+    return p;\r
+}\r
+\r
+\r
+void StrPair::CollapseWhitespace()\r
+{\r
+    // Adjusting _start would cause undefined behavior on delete[]\r
+    TIXMLASSERT( ( _flags & NEEDS_DELETE ) == 0 );\r
+    // Trim leading space.\r
+    _start = XMLUtil::SkipWhiteSpace( _start, 0 );\r
+\r
+    if ( *_start ) {\r
+        const char* p = _start;        // the read pointer\r
+        char* q = _start;      // the write pointer\r
+\r
+        while( *p ) {\r
+            if ( XMLUtil::IsWhiteSpace( *p )) {\r
+                p = XMLUtil::SkipWhiteSpace( p, 0 );\r
+                if ( *p == 0 ) {\r
+                    break;    // don't write to q; this trims the trailing space.\r
+                }\r
+                *q = ' ';\r
+                ++q;\r
+            }\r
+            *q = *p;\r
+            ++q;\r
+            ++p;\r
+        }\r
+        *q = 0;\r
+    }\r
+}\r
+\r
+\r
+const char* StrPair::GetStr()\r
+{\r
+    TIXMLASSERT( _start );\r
+    TIXMLASSERT( _end );\r
+    if ( _flags & NEEDS_FLUSH ) {\r
+        *_end = 0;\r
+        _flags ^= NEEDS_FLUSH;\r
+\r
+        if ( _flags ) {\r
+            const char* p = _start;    // the read pointer\r
+            char* q = _start;  // the write pointer\r
+\r
+            while( p < _end ) {\r
+                if ( (_flags & NEEDS_NEWLINE_NORMALIZATION) && *p == CR ) {\r
+                    // CR-LF pair becomes LF\r
+                    // CR alone becomes LF\r
+                    // LF-CR becomes LF\r
+                    if ( *(p+1) == LF ) {\r
+                        p += 2;\r
+                    }\r
+                    else {\r
+                        ++p;\r
+                    }\r
+                    *q = LF;\r
+                    ++q;\r
+                }\r
+                else if ( (_flags & NEEDS_NEWLINE_NORMALIZATION) && *p == LF ) {\r
+                    if ( *(p+1) == CR ) {\r
+                        p += 2;\r
+                    }\r
+                    else {\r
+                        ++p;\r
+                    }\r
+                    *q = LF;\r
+                    ++q;\r
+                }\r
+                else if ( (_flags & NEEDS_ENTITY_PROCESSING) && *p == '&' ) {\r
+                    // Entities handled by tinyXML2:\r
+                    // - special entities in the entity table [in/out]\r
+                    // - numeric character reference [in]\r
+                    //   &#20013; or &#x4e2d;\r
+\r
+                    if ( *(p+1) == '#' ) {\r
+                        const int buflen = 10;\r
+                        char buf[buflen] = { 0 };\r
+                        int len = 0;\r
+                        char* adjusted = const_cast<char*>( XMLUtil::GetCharacterRef( p, buf, &len ) );\r
+                        if ( adjusted == 0 ) {\r
+                            *q = *p;\r
+                            ++p;\r
+                            ++q;\r
+                        }\r
+                        else {\r
+                            TIXMLASSERT( 0 <= len && len <= buflen );\r
+                            TIXMLASSERT( q + len <= adjusted );\r
+                            p = adjusted;\r
+                            memcpy( q, buf, len );\r
+                            q += len;\r
+                        }\r
+                    }\r
+                    else {\r
+                        bool entityFound = false;\r
+                        for( int i = 0; i < NUM_ENTITIES; ++i ) {\r
+                            const Entity& entity = entities[i];\r
+                            if ( strncmp( p + 1, entity.pattern, entity.length ) == 0\r
+                                    && *( p + entity.length + 1 ) == ';' ) {\r
+                                // Found an entity - convert.\r
+                                *q = entity.value;\r
+                                ++q;\r
+                                p += entity.length + 2;\r
+                                entityFound = true;\r
+                                break;\r
+                            }\r
+                        }\r
+                        if ( !entityFound ) {\r
+                            // fixme: treat as error?\r
+                            ++p;\r
+                            ++q;\r
+                        }\r
+                    }\r
+                }\r
+                else {\r
+                    *q = *p;\r
+                    ++p;\r
+                    ++q;\r
+                }\r
+            }\r
+            *q = 0;\r
+        }\r
+        // The loop below has plenty going on, and this\r
+        // is a less useful mode. Break it out.\r
+        if ( _flags & NEEDS_WHITESPACE_COLLAPSING ) {\r
+            CollapseWhitespace();\r
+        }\r
+        _flags = (_flags & NEEDS_DELETE);\r
+    }\r
+    TIXMLASSERT( _start );\r
+    return _start;\r
+}\r
+\r
+\r
+\r
+\r
+// --------- XMLUtil ----------- //\r
+\r
+const char* XMLUtil::writeBoolTrue  = "true";\r
+const char* XMLUtil::writeBoolFalse = "false";\r
+\r
+void XMLUtil::SetBoolSerialization(const char* writeTrue, const char* writeFalse)\r
+{\r
+       static const char* defTrue  = "true";\r
+       static const char* defFalse = "false";\r
+\r
+       writeBoolTrue = (writeTrue) ? writeTrue : defTrue;\r
+       writeBoolFalse = (writeFalse) ? writeFalse : defFalse;\r
+}\r
+\r
+\r
+const char* XMLUtil::ReadBOM( const char* p, bool* bom )\r
+{\r
+    TIXMLASSERT( p );\r
+    TIXMLASSERT( bom );\r
+    *bom = false;\r
+    const unsigned char* pu = reinterpret_cast<const unsigned char*>(p);\r
+    // Check for BOM:\r
+    if (    *(pu+0) == TIXML_UTF_LEAD_0\r
+            && *(pu+1) == TIXML_UTF_LEAD_1\r
+            && *(pu+2) == TIXML_UTF_LEAD_2 ) {\r
+        *bom = true;\r
+        p += 3;\r
+    }\r
+    TIXMLASSERT( p );\r
+    return p;\r
+}\r
+\r
+\r
+void XMLUtil::ConvertUTF32ToUTF8( unsigned long input, char* output, int* length )\r
+{\r
+    const unsigned long BYTE_MASK = 0xBF;\r
+    const unsigned long BYTE_MARK = 0x80;\r
+    const unsigned long FIRST_BYTE_MARK[7] = { 0x00, 0x00, 0xC0, 0xE0, 0xF0, 0xF8, 0xFC };\r
+\r
+    if (input < 0x80) {\r
+        *length = 1;\r
+    }\r
+    else if ( input < 0x800 ) {\r
+        *length = 2;\r
+    }\r
+    else if ( input < 0x10000 ) {\r
+        *length = 3;\r
+    }\r
+    else if ( input < 0x200000 ) {\r
+        *length = 4;\r
+    }\r
+    else {\r
+        *length = 0;    // This code won't convert this correctly anyway.\r
+        return;\r
+    }\r
+\r
+    output += *length;\r
+\r
+    // Scary scary fall throughs are annotated with carefully designed comments\r
+    // to suppress compiler warnings such as -Wimplicit-fallthrough in gcc\r
+    switch (*length) {\r
+        case 4:\r
+            --output;\r
+            *output = (char)((input | BYTE_MARK) & BYTE_MASK);\r
+            input >>= 6;\r
+            //fall through\r
+        case 3:\r
+            --output;\r
+            *output = (char)((input | BYTE_MARK) & BYTE_MASK);\r
+            input >>= 6;\r
+            //fall through\r
+        case 2:\r
+            --output;\r
+            *output = (char)((input | BYTE_MARK) & BYTE_MASK);\r
+            input >>= 6;\r
+            //fall through\r
+        case 1:\r
+            --output;\r
+            *output = (char)(input | FIRST_BYTE_MARK[*length]);\r
+            break;\r
+        default:\r
+            TIXMLASSERT( false );\r
+    }\r
+}\r
+\r
+\r
+const char* XMLUtil::GetCharacterRef( const char* p, char* value, int* length )\r
+{\r
+    // Presume an entity, and pull it out.\r
+    *length = 0;\r
+\r
+    if ( *(p+1) == '#' && *(p+2) ) {\r
+        unsigned long ucs = 0;\r
+        TIXMLASSERT( sizeof( ucs ) >= 4 );\r
+        ptrdiff_t delta = 0;\r
+        unsigned mult = 1;\r
+        static const char SEMICOLON = ';';\r
+\r
+        if ( *(p+2) == 'x' ) {\r
+            // Hexadecimal.\r
+            const char* q = p+3;\r
+            if ( !(*q) ) {\r
+                return 0;\r
+            }\r
+\r
+            q = strchr( q, SEMICOLON );\r
+\r
+            if ( !q ) {\r
+                return 0;\r
+            }\r
+            TIXMLASSERT( *q == SEMICOLON );\r
+\r
+            delta = q-p;\r
+            --q;\r
+\r
+            while ( *q != 'x' ) {\r
+                unsigned int digit = 0;\r
+\r
+                if ( *q >= '0' && *q <= '9' ) {\r
+                    digit = *q - '0';\r
+                }\r
+                else if ( *q >= 'a' && *q <= 'f' ) {\r
+                    digit = *q - 'a' + 10;\r
+                }\r
+                else if ( *q >= 'A' && *q <= 'F' ) {\r
+                    digit = *q - 'A' + 10;\r
+                }\r
+                else {\r
+                    return 0;\r
+                }\r
+                TIXMLASSERT( digit < 16 );\r
+                TIXMLASSERT( digit == 0 || mult <= UINT_MAX / digit );\r
+                const unsigned int digitScaled = mult * digit;\r
+                TIXMLASSERT( ucs <= ULONG_MAX - digitScaled );\r
+                ucs += digitScaled;\r
+                TIXMLASSERT( mult <= UINT_MAX / 16 );\r
+                mult *= 16;\r
+                --q;\r
+            }\r
+        }\r
+        else {\r
+            // Decimal.\r
+            const char* q = p+2;\r
+            if ( !(*q) ) {\r
+                return 0;\r
+            }\r
+\r
+            q = strchr( q, SEMICOLON );\r
+\r
+            if ( !q ) {\r
+                return 0;\r
+            }\r
+            TIXMLASSERT( *q == SEMICOLON );\r
+\r
+            delta = q-p;\r
+            --q;\r
+\r
+            while ( *q != '#' ) {\r
+                if ( *q >= '0' && *q <= '9' ) {\r
+                    const unsigned int digit = *q - '0';\r
+                    TIXMLASSERT( digit < 10 );\r
+                    TIXMLASSERT( digit == 0 || mult <= UINT_MAX / digit );\r
+                    const unsigned int digitScaled = mult * digit;\r
+                    TIXMLASSERT( ucs <= ULONG_MAX - digitScaled );\r
+                    ucs += digitScaled;\r
+                }\r
+                else {\r
+                    return 0;\r
+                }\r
+                TIXMLASSERT( mult <= UINT_MAX / 10 );\r
+                mult *= 10;\r
+                --q;\r
+            }\r
+        }\r
+        // convert the UCS to UTF-8\r
+        ConvertUTF32ToUTF8( ucs, value, length );\r
+        return p + delta + 1;\r
+    }\r
+    return p+1;\r
+}\r
+\r
+\r
+void XMLUtil::ToStr( int v, char* buffer, int bufferSize )\r
+{\r
+    TIXML_SNPRINTF( buffer, bufferSize, "%d", v );\r
+}\r
+\r
+\r
+void XMLUtil::ToStr( unsigned v, char* buffer, int bufferSize )\r
+{\r
+    TIXML_SNPRINTF( buffer, bufferSize, "%u", v );\r
+}\r
+\r
+\r
+void XMLUtil::ToStr( bool v, char* buffer, int bufferSize )\r
+{\r
+    TIXML_SNPRINTF( buffer, bufferSize, "%s", v ? writeBoolTrue : writeBoolFalse);\r
+}\r
+\r
+/*\r
+       ToStr() of a number is a very tricky topic.\r
+       https://github.com/leethomason/tinyxml2/issues/106\r
+*/\r
+void XMLUtil::ToStr( float v, char* buffer, int bufferSize )\r
+{\r
+    TIXML_SNPRINTF( buffer, bufferSize, "%.8g", v );\r
+}\r
+\r
+\r
+void XMLUtil::ToStr( double v, char* buffer, int bufferSize )\r
+{\r
+    TIXML_SNPRINTF( buffer, bufferSize, "%.17g", v );\r
+}\r
+\r
+\r
+void XMLUtil::ToStr(int64_t v, char* buffer, int bufferSize)\r
+{\r
+       // horrible syntax trick to make the compiler happy about %lld\r
+       TIXML_SNPRINTF(buffer, bufferSize, "%lld", (long long)v);\r
+}\r
+\r
+\r
+bool XMLUtil::ToInt( const char* str, int* value )\r
+{\r
+    if ( TIXML_SSCANF( str, "%d", value ) == 1 ) {\r
+        return true;\r
+    }\r
+    return false;\r
+}\r
+\r
+bool XMLUtil::ToUnsigned( const char* str, unsigned *value )\r
+{\r
+    if ( TIXML_SSCANF( str, "%u", value ) == 1 ) {\r
+        return true;\r
+    }\r
+    return false;\r
+}\r
+\r
+bool XMLUtil::ToBool( const char* str, bool* value )\r
+{\r
+    int ival = 0;\r
+    if ( ToInt( str, &ival )) {\r
+        *value = (ival==0) ? false : true;\r
+        return true;\r
+    }\r
+    if ( StringEqual( str, "true" ) ) {\r
+        *value = true;\r
+        return true;\r
+    }\r
+    else if ( StringEqual( str, "false" ) ) {\r
+        *value = false;\r
+        return true;\r
+    }\r
+    return false;\r
+}\r
+\r
+\r
+bool XMLUtil::ToFloat( const char* str, float* value )\r
+{\r
+    if ( TIXML_SSCANF( str, "%f", value ) == 1 ) {\r
+        return true;\r
+    }\r
+    return false;\r
+}\r
+\r
+\r
+bool XMLUtil::ToDouble( const char* str, double* value )\r
+{\r
+    if ( TIXML_SSCANF( str, "%lf", value ) == 1 ) {\r
+        return true;\r
+    }\r
+    return false;\r
+}\r
+\r
+\r
+bool XMLUtil::ToInt64(const char* str, int64_t* value)\r
+{\r
+       long long v = 0;        // horrible syntax trick to make the compiler happy about %lld\r
+       if (TIXML_SSCANF(str, "%lld", &v) == 1) {\r
+               *value = (int64_t)v;\r
+               return true;\r
+       }\r
+       return false;\r
+}\r
+\r
+\r
+char* XMLDocument::Identify( char* p, XMLNode** node )\r
+{\r
+    TIXMLASSERT( node );\r
+    TIXMLASSERT( p );\r
+    char* const start = p;\r
+    int const startLine = _parseCurLineNum;\r
+    p = XMLUtil::SkipWhiteSpace( p, &_parseCurLineNum );\r
+    if( !*p ) {\r
+        *node = 0;\r
+        TIXMLASSERT( p );\r
+        return p;\r
+    }\r
+\r
+    // These strings define the matching patterns:\r
+    static const char* xmlHeader               = { "<?" };\r
+    static const char* commentHeader   = { "<!--" };\r
+    static const char* cdataHeader             = { "<![CDATA[" };\r
+    static const char* dtdHeader               = { "<!" };\r
+    static const char* elementHeader   = { "<" };      // and a header for everything else; check last.\r
+\r
+    static const int xmlHeaderLen              = 2;\r
+    static const int commentHeaderLen  = 4;\r
+    static const int cdataHeaderLen            = 9;\r
+    static const int dtdHeaderLen              = 2;\r
+    static const int elementHeaderLen  = 1;\r
+\r
+    TIXMLASSERT( sizeof( XMLComment ) == sizeof( XMLUnknown ) );               // use same memory pool\r
+    TIXMLASSERT( sizeof( XMLComment ) == sizeof( XMLDeclaration ) );   // use same memory pool\r
+    XMLNode* returnNode = 0;\r
+    if ( XMLUtil::StringEqual( p, xmlHeader, xmlHeaderLen ) ) {\r
+        returnNode = CreateUnlinkedNode<XMLDeclaration>( _commentPool );\r
+        returnNode->_parseLineNum = _parseCurLineNum;\r
+        p += xmlHeaderLen;\r
+    }\r
+    else if ( XMLUtil::StringEqual( p, commentHeader, commentHeaderLen ) ) {\r
+        returnNode = CreateUnlinkedNode<XMLComment>( _commentPool );\r
+        returnNode->_parseLineNum = _parseCurLineNum;\r
+        p += commentHeaderLen;\r
+    }\r
+    else if ( XMLUtil::StringEqual( p, cdataHeader, cdataHeaderLen ) ) {\r
+        XMLText* text = CreateUnlinkedNode<XMLText>( _textPool );\r
+        returnNode = text;\r
+        returnNode->_parseLineNum = _parseCurLineNum;\r
+        p += cdataHeaderLen;\r
+        text->SetCData( true );\r
+    }\r
+    else if ( XMLUtil::StringEqual( p, dtdHeader, dtdHeaderLen ) ) {\r
+        returnNode = CreateUnlinkedNode<XMLUnknown>( _commentPool );\r
+        returnNode->_parseLineNum = _parseCurLineNum;\r
+        p += dtdHeaderLen;\r
+    }\r
+    else if ( XMLUtil::StringEqual( p, elementHeader, elementHeaderLen ) ) {\r
+        returnNode =  CreateUnlinkedNode<XMLElement>( _elementPool );\r
+        returnNode->_parseLineNum = _parseCurLineNum;\r
+        p += elementHeaderLen;\r
+    }\r
+    else {\r
+        returnNode = CreateUnlinkedNode<XMLText>( _textPool );\r
+        returnNode->_parseLineNum = _parseCurLineNum; // Report line of first non-whitespace character\r
+        p = start;     // Back it up, all the text counts.\r
+        _parseCurLineNum = startLine;\r
+    }\r
+\r
+    TIXMLASSERT( returnNode );\r
+    TIXMLASSERT( p );\r
+    *node = returnNode;\r
+    return p;\r
+}\r
+\r
+\r
+bool XMLDocument::Accept( XMLVisitor* visitor ) const\r
+{\r
+    TIXMLASSERT( visitor );\r
+    if ( visitor->VisitEnter( *this ) ) {\r
+        for ( const XMLNode* node=FirstChild(); node; node=node->NextSibling() ) {\r
+            if ( !node->Accept( visitor ) ) {\r
+                break;\r
+            }\r
+        }\r
+    }\r
+    return visitor->VisitExit( *this );\r
+}\r
+\r
+\r
+// --------- XMLNode ----------- //\r
+\r
+XMLNode::XMLNode( XMLDocument* doc ) :\r
+    _document( doc ),\r
+    _parent( 0 ),\r
+    _value(),\r
+    _parseLineNum( 0 ),\r
+    _firstChild( 0 ), _lastChild( 0 ),\r
+    _prev( 0 ), _next( 0 ),\r
+       _userData( 0 ),\r
+    _memPool( 0 )\r
+{\r
+}\r
+\r
+\r
+XMLNode::~XMLNode()\r
+{\r
+    DeleteChildren();\r
+    if ( _parent ) {\r
+        _parent->Unlink( this );\r
+    }\r
+}\r
+\r
+const char* XMLNode::Value() const\r
+{\r
+    // Edge case: XMLDocuments don't have a Value. Return null.\r
+    if ( this->ToDocument() )\r
+        return 0;\r
+    return _value.GetStr();\r
+}\r
+\r
+void XMLNode::SetValue( const char* str, bool staticMem )\r
+{\r
+    if ( staticMem ) {\r
+        _value.SetInternedStr( str );\r
+    }\r
+    else {\r
+        _value.SetStr( str );\r
+    }\r
+}\r
+\r
+XMLNode* XMLNode::DeepClone(XMLDocument* target) const\r
+{\r
+       XMLNode* clone = this->ShallowClone(target);\r
+       if (!clone) return 0;\r
+\r
+       for (const XMLNode* child = this->FirstChild(); child; child = child->NextSibling()) {\r
+               XMLNode* childClone = child->DeepClone(target);\r
+               TIXMLASSERT(childClone);\r
+               clone->InsertEndChild(childClone);\r
+       }\r
+       return clone;\r
+}\r
+\r
+void XMLNode::DeleteChildren()\r
+{\r
+    while( _firstChild ) {\r
+        TIXMLASSERT( _lastChild );\r
+        DeleteChild( _firstChild );\r
+    }\r
+    _firstChild = _lastChild = 0;\r
+}\r
+\r
+\r
+void XMLNode::Unlink( XMLNode* child )\r
+{\r
+    TIXMLASSERT( child );\r
+    TIXMLASSERT( child->_document == _document );\r
+    TIXMLASSERT( child->_parent == this );\r
+    if ( child == _firstChild ) {\r
+        _firstChild = _firstChild->_next;\r
+    }\r
+    if ( child == _lastChild ) {\r
+        _lastChild = _lastChild->_prev;\r
+    }\r
+\r
+    if ( child->_prev ) {\r
+        child->_prev->_next = child->_next;\r
+    }\r
+    if ( child->_next ) {\r
+        child->_next->_prev = child->_prev;\r
+    }\r
+       child->_next = 0;\r
+       child->_prev = 0;\r
+       child->_parent = 0;\r
+}\r
+\r
+\r
+void XMLNode::DeleteChild( XMLNode* node )\r
+{\r
+    TIXMLASSERT( node );\r
+    TIXMLASSERT( node->_document == _document );\r
+    TIXMLASSERT( node->_parent == this );\r
+    Unlink( node );\r
+       TIXMLASSERT(node->_prev == 0);\r
+       TIXMLASSERT(node->_next == 0);\r
+       TIXMLASSERT(node->_parent == 0);\r
+    DeleteNode( node );\r
+}\r
+\r
+\r
+XMLNode* XMLNode::InsertEndChild( XMLNode* addThis )\r
+{\r
+    TIXMLASSERT( addThis );\r
+    if ( addThis->_document != _document ) {\r
+        TIXMLASSERT( false );\r
+        return 0;\r
+    }\r
+    InsertChildPreamble( addThis );\r
+\r
+    if ( _lastChild ) {\r
+        TIXMLASSERT( _firstChild );\r
+        TIXMLASSERT( _lastChild->_next == 0 );\r
+        _lastChild->_next = addThis;\r
+        addThis->_prev = _lastChild;\r
+        _lastChild = addThis;\r
+\r
+        addThis->_next = 0;\r
+    }\r
+    else {\r
+        TIXMLASSERT( _firstChild == 0 );\r
+        _firstChild = _lastChild = addThis;\r
+\r
+        addThis->_prev = 0;\r
+        addThis->_next = 0;\r
+    }\r
+    addThis->_parent = this;\r
+    return addThis;\r
+}\r
+\r
+\r
+XMLNode* XMLNode::InsertFirstChild( XMLNode* addThis )\r
+{\r
+    TIXMLASSERT( addThis );\r
+    if ( addThis->_document != _document ) {\r
+        TIXMLASSERT( false );\r
+        return 0;\r
+    }\r
+    InsertChildPreamble( addThis );\r
+\r
+    if ( _firstChild ) {\r
+        TIXMLASSERT( _lastChild );\r
+        TIXMLASSERT( _firstChild->_prev == 0 );\r
+\r
+        _firstChild->_prev = addThis;\r
+        addThis->_next = _firstChild;\r
+        _firstChild = addThis;\r
+\r
+        addThis->_prev = 0;\r
+    }\r
+    else {\r
+        TIXMLASSERT( _lastChild == 0 );\r
+        _firstChild = _lastChild = addThis;\r
+\r
+        addThis->_prev = 0;\r
+        addThis->_next = 0;\r
+    }\r
+    addThis->_parent = this;\r
+    return addThis;\r
+}\r
+\r
+\r
+XMLNode* XMLNode::InsertAfterChild( XMLNode* afterThis, XMLNode* addThis )\r
+{\r
+    TIXMLASSERT( addThis );\r
+    if ( addThis->_document != _document ) {\r
+        TIXMLASSERT( false );\r
+        return 0;\r
+    }\r
+\r
+    TIXMLASSERT( afterThis );\r
+\r
+    if ( afterThis->_parent != this ) {\r
+        TIXMLASSERT( false );\r
+        return 0;\r
+    }\r
+    if ( afterThis == addThis ) {\r
+        // Current state: BeforeThis -> AddThis -> OneAfterAddThis\r
+        // Now AddThis must disappear from it's location and then\r
+        // reappear between BeforeThis and OneAfterAddThis.\r
+        // So just leave it where it is.\r
+        return addThis;\r
+    }\r
+\r
+    if ( afterThis->_next == 0 ) {\r
+        // The last node or the only node.\r
+        return InsertEndChild( addThis );\r
+    }\r
+    InsertChildPreamble( addThis );\r
+    addThis->_prev = afterThis;\r
+    addThis->_next = afterThis->_next;\r
+    afterThis->_next->_prev = addThis;\r
+    afterThis->_next = addThis;\r
+    addThis->_parent = this;\r
+    return addThis;\r
+}\r
+\r
+\r
+\r
+\r
+const XMLElement* XMLNode::FirstChildElement( const char* name ) const\r
+{\r
+    for( const XMLNode* node = _firstChild; node; node = node->_next ) {\r
+        const XMLElement* element = node->ToElementWithName( name );\r
+        if ( element ) {\r
+            return element;\r
+        }\r
+    }\r
+    return 0;\r
+}\r
+\r
+\r
+const XMLElement* XMLNode::LastChildElement( const char* name ) const\r
+{\r
+    for( const XMLNode* node = _lastChild; node; node = node->_prev ) {\r
+        const XMLElement* element = node->ToElementWithName( name );\r
+        if ( element ) {\r
+            return element;\r
+        }\r
+    }\r
+    return 0;\r
+}\r
+\r
+\r
+const XMLElement* XMLNode::NextSiblingElement( const char* name ) const\r
+{\r
+    for( const XMLNode* node = _next; node; node = node->_next ) {\r
+        const XMLElement* element = node->ToElementWithName( name );\r
+        if ( element ) {\r
+            return element;\r
+        }\r
+    }\r
+    return 0;\r
+}\r
+\r
+\r
+const XMLElement* XMLNode::PreviousSiblingElement( const char* name ) const\r
+{\r
+    for( const XMLNode* node = _prev; node; node = node->_prev ) {\r
+        const XMLElement* element = node->ToElementWithName( name );\r
+        if ( element ) {\r
+            return element;\r
+        }\r
+    }\r
+    return 0;\r
+}\r
+\r
+\r
+char* XMLNode::ParseDeep( char* p, StrPair* parentEndTag, int* curLineNumPtr )\r
+{\r
+    // This is a recursive method, but thinking about it "at the current level"\r
+    // it is a pretty simple flat list:\r
+    //         <foo/>\r
+    //         <!-- comment -->\r
+    //\r
+    // With a special case:\r
+    //         <foo>\r
+    //         </foo>\r
+    //         <!-- comment -->\r
+    //\r
+    // Where the closing element (/foo) *must* be the next thing after the opening\r
+    // element, and the names must match. BUT the tricky bit is that the closing\r
+    // element will be read by the child.\r
+    //\r
+    // 'endTag' is the end tag for this node, it is returned by a call to a child.\r
+    // 'parentEnd' is the end tag for the parent, which is filled in and returned.\r
+\r
+       XMLDocument::DepthTracker tracker(_document);\r
+       if (_document->Error())\r
+               return 0;\r
+\r
+       while( p && *p ) {\r
+        XMLNode* node = 0;\r
+\r
+        p = _document->Identify( p, &node );\r
+        TIXMLASSERT( p );\r
+        if ( node == 0 ) {\r
+            break;\r
+        }\r
+\r
+        int initialLineNum = node->_parseLineNum;\r
+\r
+        StrPair endTag;\r
+        p = node->ParseDeep( p, &endTag, curLineNumPtr );\r
+        if ( !p ) {\r
+            DeleteNode( node );\r
+            if ( !_document->Error() ) {\r
+                _document->SetError( XML_ERROR_PARSING, initialLineNum, 0);\r
+            }\r
+            break;\r
+        }\r
+\r
+        XMLDeclaration* decl = node->ToDeclaration();\r
+        if ( decl ) {\r
+            // Declarations are only allowed at document level\r
+            //\r
+            // Multiple declarations are allowed but all declarations\r
+            // must occur before anything else. \r
+            //\r
+            // Optimized due to a security test case. If the first node is \r
+            // a declaration, and the last node is a declaration, then only \r
+            // declarations have so far been addded.\r
+            bool wellLocated = false;\r
+\r
+            if (ToDocument()) {\r
+                if (FirstChild()) {\r
+                    wellLocated =\r
+                        FirstChild() &&\r
+                        FirstChild()->ToDeclaration() &&\r
+                        LastChild() &&\r
+                        LastChild()->ToDeclaration();\r
+                }\r
+                else {\r
+                    wellLocated = true;\r
+                }\r
+            }\r
+            if ( !wellLocated ) {\r
+                _document->SetError( XML_ERROR_PARSING_DECLARATION, initialLineNum, "XMLDeclaration value=%s", decl->Value());\r
+                DeleteNode( node );\r
+                break;\r
+            }\r
+        }\r
+\r
+        XMLElement* ele = node->ToElement();\r
+        if ( ele ) {\r
+            // We read the end tag. Return it to the parent.\r
+            if ( ele->ClosingType() == XMLElement::CLOSING ) {\r
+                if ( parentEndTag ) {\r
+                    ele->_value.TransferTo( parentEndTag );\r
+                }\r
+                node->_memPool->SetTracked();   // created and then immediately deleted.\r
+                DeleteNode( node );\r
+                return p;\r
+            }\r
+\r
+            // Handle an end tag returned to this level.\r
+            // And handle a bunch of annoying errors.\r
+            bool mismatch = false;\r
+            if ( endTag.Empty() ) {\r
+                if ( ele->ClosingType() == XMLElement::OPEN ) {\r
+                    mismatch = true;\r
+                }\r
+            }\r
+            else {\r
+                if ( ele->ClosingType() != XMLElement::OPEN ) {\r
+                    mismatch = true;\r
+                }\r
+                else if ( !XMLUtil::StringEqual( endTag.GetStr(), ele->Name() ) ) {\r
+                    mismatch = true;\r
+                }\r
+            }\r
+            if ( mismatch ) {\r
+                _document->SetError( XML_ERROR_MISMATCHED_ELEMENT, initialLineNum, "XMLElement name=%s", ele->Name());\r
+                DeleteNode( node );\r
+                break;\r
+            }\r
+        }\r
+        InsertEndChild( node );\r
+    }\r
+    return 0;\r
+}\r
+\r
+/*static*/ void XMLNode::DeleteNode( XMLNode* node )\r
+{\r
+    if ( node == 0 ) {\r
+        return;\r
+    }\r
+       TIXMLASSERT(node->_document);\r
+       if (!node->ToDocument()) {\r
+               node->_document->MarkInUse(node);\r
+       }\r
+\r
+    MemPool* pool = node->_memPool;\r
+    node->~XMLNode();\r
+    pool->Free( node );\r
+}\r
+\r
+void XMLNode::InsertChildPreamble( XMLNode* insertThis ) const\r
+{\r
+    TIXMLASSERT( insertThis );\r
+    TIXMLASSERT( insertThis->_document == _document );\r
+\r
+       if (insertThis->_parent) {\r
+        insertThis->_parent->Unlink( insertThis );\r
+       }\r
+       else {\r
+               insertThis->_document->MarkInUse(insertThis);\r
+        insertThis->_memPool->SetTracked();\r
+       }\r
+}\r
+\r
+const XMLElement* XMLNode::ToElementWithName( const char* name ) const\r
+{\r
+    const XMLElement* element = this->ToElement();\r
+    if ( element == 0 ) {\r
+        return 0;\r
+    }\r
+    if ( name == 0 ) {\r
+        return element;\r
+    }\r
+    if ( XMLUtil::StringEqual( element->Name(), name ) ) {\r
+       return element;\r
+    }\r
+    return 0;\r
+}\r
+\r
+// --------- XMLText ---------- //\r
+char* XMLText::ParseDeep( char* p, StrPair*, int* curLineNumPtr )\r
+{\r
+    if ( this->CData() ) {\r
+        p = _value.ParseText( p, "]]>", StrPair::NEEDS_NEWLINE_NORMALIZATION, curLineNumPtr );\r
+        if ( !p ) {\r
+            _document->SetError( XML_ERROR_PARSING_CDATA, _parseLineNum, 0 );\r
+        }\r
+        return p;\r
+    }\r
+    else {\r
+        int flags = _document->ProcessEntities() ? StrPair::TEXT_ELEMENT : StrPair::TEXT_ELEMENT_LEAVE_ENTITIES;\r
+        if ( _document->WhitespaceMode() == COLLAPSE_WHITESPACE ) {\r
+            flags |= StrPair::NEEDS_WHITESPACE_COLLAPSING;\r
+        }\r
+\r
+        p = _value.ParseText( p, "<", flags, curLineNumPtr );\r
+        if ( p && *p ) {\r
+            return p-1;\r
+        }\r
+        if ( !p ) {\r
+            _document->SetError( XML_ERROR_PARSING_TEXT, _parseLineNum, 0 );\r
+        }\r
+    }\r
+    return 0;\r
+}\r
+\r
+\r
+XMLNode* XMLText::ShallowClone( XMLDocument* doc ) const\r
+{\r
+    if ( !doc ) {\r
+        doc = _document;\r
+    }\r
+    XMLText* text = doc->NewText( Value() );   // fixme: this will always allocate memory. Intern?\r
+    text->SetCData( this->CData() );\r
+    return text;\r
+}\r
+\r
+\r
+bool XMLText::ShallowEqual( const XMLNode* compare ) const\r
+{\r
+    TIXMLASSERT( compare );\r
+    const XMLText* text = compare->ToText();\r
+    return ( text && XMLUtil::StringEqual( text->Value(), Value() ) );\r
+}\r
+\r
+\r
+bool XMLText::Accept( XMLVisitor* visitor ) const\r
+{\r
+    TIXMLASSERT( visitor );\r
+    return visitor->Visit( *this );\r
+}\r
+\r
+\r
+// --------- XMLComment ---------- //\r
+\r
+XMLComment::XMLComment( XMLDocument* doc ) : XMLNode( doc )\r
+{\r
+}\r
+\r
+\r
+XMLComment::~XMLComment()\r
+{\r
+}\r
+\r
+\r
+char* XMLComment::ParseDeep( char* p, StrPair*, int* curLineNumPtr )\r
+{\r
+    // Comment parses as text.\r
+    p = _value.ParseText( p, "-->", StrPair::COMMENT, curLineNumPtr );\r
+    if ( p == 0 ) {\r
+        _document->SetError( XML_ERROR_PARSING_COMMENT, _parseLineNum, 0 );\r
+    }\r
+    return p;\r
+}\r
+\r
+\r
+XMLNode* XMLComment::ShallowClone( XMLDocument* doc ) const\r
+{\r
+    if ( !doc ) {\r
+        doc = _document;\r
+    }\r
+    XMLComment* comment = doc->NewComment( Value() );  // fixme: this will always allocate memory. Intern?\r
+    return comment;\r
+}\r
+\r
+\r
+bool XMLComment::ShallowEqual( const XMLNode* compare ) const\r
+{\r
+    TIXMLASSERT( compare );\r
+    const XMLComment* comment = compare->ToComment();\r
+    return ( comment && XMLUtil::StringEqual( comment->Value(), Value() ));\r
+}\r
+\r
+\r
+bool XMLComment::Accept( XMLVisitor* visitor ) const\r
+{\r
+    TIXMLASSERT( visitor );\r
+    return visitor->Visit( *this );\r
+}\r
+\r
+\r
+// --------- XMLDeclaration ---------- //\r
+\r
+XMLDeclaration::XMLDeclaration( XMLDocument* doc ) : XMLNode( doc )\r
+{\r
+}\r
+\r
+\r
+XMLDeclaration::~XMLDeclaration()\r
+{\r
+    //printf( "~XMLDeclaration\n" );\r
+}\r
+\r
+\r
+char* XMLDeclaration::ParseDeep( char* p, StrPair*, int* curLineNumPtr )\r
+{\r
+    // Declaration parses as text.\r
+    p = _value.ParseText( p, "?>", StrPair::NEEDS_NEWLINE_NORMALIZATION, curLineNumPtr );\r
+    if ( p == 0 ) {\r
+        _document->SetError( XML_ERROR_PARSING_DECLARATION, _parseLineNum, 0 );\r
+    }\r
+    return p;\r
+}\r
+\r
+\r
+XMLNode* XMLDeclaration::ShallowClone( XMLDocument* doc ) const\r
+{\r
+    if ( !doc ) {\r
+        doc = _document;\r
+    }\r
+    XMLDeclaration* dec = doc->NewDeclaration( Value() );      // fixme: this will always allocate memory. Intern?\r
+    return dec;\r
+}\r
+\r
+\r
+bool XMLDeclaration::ShallowEqual( const XMLNode* compare ) const\r
+{\r
+    TIXMLASSERT( compare );\r
+    const XMLDeclaration* declaration = compare->ToDeclaration();\r
+    return ( declaration && XMLUtil::StringEqual( declaration->Value(), Value() ));\r
+}\r
+\r
+\r
+\r
+bool XMLDeclaration::Accept( XMLVisitor* visitor ) const\r
+{\r
+    TIXMLASSERT( visitor );\r
+    return visitor->Visit( *this );\r
+}\r
+\r
+// --------- XMLUnknown ---------- //\r
+\r
+XMLUnknown::XMLUnknown( XMLDocument* doc ) : XMLNode( doc )\r
+{\r
+}\r
+\r
+\r
+XMLUnknown::~XMLUnknown()\r
+{\r
+}\r
+\r
+\r
+char* XMLUnknown::ParseDeep( char* p, StrPair*, int* curLineNumPtr )\r
+{\r
+    // Unknown parses as text.\r
+    p = _value.ParseText( p, ">", StrPair::NEEDS_NEWLINE_NORMALIZATION, curLineNumPtr );\r
+    if ( !p ) {\r
+        _document->SetError( XML_ERROR_PARSING_UNKNOWN, _parseLineNum, 0 );\r
+    }\r
+    return p;\r
+}\r
+\r
+\r
+XMLNode* XMLUnknown::ShallowClone( XMLDocument* doc ) const\r
+{\r
+    if ( !doc ) {\r
+        doc = _document;\r
+    }\r
+    XMLUnknown* text = doc->NewUnknown( Value() );     // fixme: this will always allocate memory. Intern?\r
+    return text;\r
+}\r
+\r
+\r
+bool XMLUnknown::ShallowEqual( const XMLNode* compare ) const\r
+{\r
+    TIXMLASSERT( compare );\r
+    const XMLUnknown* unknown = compare->ToUnknown();\r
+    return ( unknown && XMLUtil::StringEqual( unknown->Value(), Value() ));\r
+}\r
+\r
+\r
+bool XMLUnknown::Accept( XMLVisitor* visitor ) const\r
+{\r
+    TIXMLASSERT( visitor );\r
+    return visitor->Visit( *this );\r
+}\r
+\r
+// --------- XMLAttribute ---------- //\r
+\r
+const char* XMLAttribute::Name() const\r
+{\r
+    return _name.GetStr();\r
+}\r
+\r
+const char* XMLAttribute::Value() const\r
+{\r
+    return _value.GetStr();\r
+}\r
+\r
+char* XMLAttribute::ParseDeep( char* p, bool processEntities, int* curLineNumPtr )\r
+{\r
+    // Parse using the name rules: bug fix, was using ParseText before\r
+    p = _name.ParseName( p );\r
+    if ( !p || !*p ) {\r
+        return 0;\r
+    }\r
+\r
+    // Skip white space before =\r
+    p = XMLUtil::SkipWhiteSpace( p, curLineNumPtr );\r
+    if ( *p != '=' ) {\r
+        return 0;\r
+    }\r
+\r
+    ++p;       // move up to opening quote\r
+    p = XMLUtil::SkipWhiteSpace( p, curLineNumPtr );\r
+    if ( *p != '\"' && *p != '\'' ) {\r
+        return 0;\r
+    }\r
+\r
+    char endTag[2] = { *p, 0 };\r
+    ++p;       // move past opening quote\r
+\r
+    p = _value.ParseText( p, endTag, processEntities ? StrPair::ATTRIBUTE_VALUE : StrPair::ATTRIBUTE_VALUE_LEAVE_ENTITIES, curLineNumPtr );\r
+    return p;\r
+}\r
+\r
+\r
+void XMLAttribute::SetName( const char* n )\r
+{\r
+    _name.SetStr( n );\r
+}\r
+\r
+\r
+XMLError XMLAttribute::QueryIntValue( int* value ) const\r
+{\r
+    if ( XMLUtil::ToInt( Value(), value )) {\r
+        return XML_SUCCESS;\r
+    }\r
+    return XML_WRONG_ATTRIBUTE_TYPE;\r
+}\r
+\r
+\r
+XMLError XMLAttribute::QueryUnsignedValue( unsigned int* value ) const\r
+{\r
+    if ( XMLUtil::ToUnsigned( Value(), value )) {\r
+        return XML_SUCCESS;\r
+    }\r
+    return XML_WRONG_ATTRIBUTE_TYPE;\r
+}\r
+\r
+\r
+XMLError XMLAttribute::QueryInt64Value(int64_t* value) const\r
+{\r
+       if (XMLUtil::ToInt64(Value(), value)) {\r
+               return XML_SUCCESS;\r
+       }\r
+       return XML_WRONG_ATTRIBUTE_TYPE;\r
+}\r
+\r
+\r
+XMLError XMLAttribute::QueryBoolValue( bool* value ) const\r
+{\r
+    if ( XMLUtil::ToBool( Value(), value )) {\r
+        return XML_SUCCESS;\r
+    }\r
+    return XML_WRONG_ATTRIBUTE_TYPE;\r
+}\r
+\r
+\r
+XMLError XMLAttribute::QueryFloatValue( float* value ) const\r
+{\r
+    if ( XMLUtil::ToFloat( Value(), value )) {\r
+        return XML_SUCCESS;\r
+    }\r
+    return XML_WRONG_ATTRIBUTE_TYPE;\r
+}\r
+\r
+\r
+XMLError XMLAttribute::QueryDoubleValue( double* value ) const\r
+{\r
+    if ( XMLUtil::ToDouble( Value(), value )) {\r
+        return XML_SUCCESS;\r
+    }\r
+    return XML_WRONG_ATTRIBUTE_TYPE;\r
+}\r
+\r
+\r
+void XMLAttribute::SetAttribute( const char* v )\r
+{\r
+    _value.SetStr( v );\r
+}\r
+\r
+\r
+void XMLAttribute::SetAttribute( int v )\r
+{\r
+    char buf[BUF_SIZE];\r
+    XMLUtil::ToStr( v, buf, BUF_SIZE );\r
+    _value.SetStr( buf );\r
+}\r
+\r
+\r
+void XMLAttribute::SetAttribute( unsigned v )\r
+{\r
+    char buf[BUF_SIZE];\r
+    XMLUtil::ToStr( v, buf, BUF_SIZE );\r
+    _value.SetStr( buf );\r
+}\r
+\r
+\r
+void XMLAttribute::SetAttribute(int64_t v)\r
+{\r
+       char buf[BUF_SIZE];\r
+       XMLUtil::ToStr(v, buf, BUF_SIZE);\r
+       _value.SetStr(buf);\r
+}\r
+\r
+\r
+\r
+void XMLAttribute::SetAttribute( bool v )\r
+{\r
+    char buf[BUF_SIZE];\r
+    XMLUtil::ToStr( v, buf, BUF_SIZE );\r
+    _value.SetStr( buf );\r
+}\r
+\r
+void XMLAttribute::SetAttribute( double v )\r
+{\r
+    char buf[BUF_SIZE];\r
+    XMLUtil::ToStr( v, buf, BUF_SIZE );\r
+    _value.SetStr( buf );\r
+}\r
+\r
+void XMLAttribute::SetAttribute( float v )\r
+{\r
+    char buf[BUF_SIZE];\r
+    XMLUtil::ToStr( v, buf, BUF_SIZE );\r
+    _value.SetStr( buf );\r
+}\r
+\r
+\r
+// --------- XMLElement ---------- //\r
+XMLElement::XMLElement( XMLDocument* doc ) : XMLNode( doc ),\r
+    _closingType( OPEN ),\r
+    _rootAttribute( 0 )\r
+{\r
+}\r
+\r
+\r
+XMLElement::~XMLElement()\r
+{\r
+    while( _rootAttribute ) {\r
+        XMLAttribute* next = _rootAttribute->_next;\r
+        DeleteAttribute( _rootAttribute );\r
+        _rootAttribute = next;\r
+    }\r
+}\r
+\r
+\r
+const XMLAttribute* XMLElement::FindAttribute( const char* name ) const\r
+{\r
+    for( XMLAttribute* a = _rootAttribute; a; a = a->_next ) {\r
+        if ( XMLUtil::StringEqual( a->Name(), name ) ) {\r
+            return a;\r
+        }\r
+    }\r
+    return 0;\r
+}\r
+\r
+\r
+const char* XMLElement::Attribute( const char* name, const char* value ) const\r
+{\r
+    const XMLAttribute* a = FindAttribute( name );\r
+    if ( !a ) {\r
+        return 0;\r
+    }\r
+    if ( !value || XMLUtil::StringEqual( a->Value(), value )) {\r
+        return a->Value();\r
+    }\r
+    return 0;\r
+}\r
+\r
+int XMLElement::IntAttribute(const char* name, int defaultValue) const\r
+{\r
+       int i = defaultValue;\r
+       QueryIntAttribute(name, &i);\r
+       return i;\r
+}\r
+\r
+unsigned XMLElement::UnsignedAttribute(const char* name, unsigned defaultValue) const\r
+{\r
+       unsigned i = defaultValue;\r
+       QueryUnsignedAttribute(name, &i);\r
+       return i;\r
+}\r
+\r
+int64_t XMLElement::Int64Attribute(const char* name, int64_t defaultValue) const\r
+{\r
+       int64_t i = defaultValue;\r
+       QueryInt64Attribute(name, &i);\r
+       return i;\r
+}\r
+\r
+bool XMLElement::BoolAttribute(const char* name, bool defaultValue) const\r
+{\r
+       bool b = defaultValue;\r
+       QueryBoolAttribute(name, &b);\r
+       return b;\r
+}\r
+\r
+double XMLElement::DoubleAttribute(const char* name, double defaultValue) const\r
+{\r
+       double d = defaultValue;\r
+       QueryDoubleAttribute(name, &d);\r
+       return d;\r
+}\r
+\r
+float XMLElement::FloatAttribute(const char* name, float defaultValue) const\r
+{\r
+       float f = defaultValue;\r
+       QueryFloatAttribute(name, &f);\r
+       return f;\r
+}\r
+\r
+const char* XMLElement::GetText() const\r
+{\r
+    if ( FirstChild() && FirstChild()->ToText() ) {\r
+        return FirstChild()->Value();\r
+    }\r
+    return 0;\r
+}\r
+\r
+\r
+void   XMLElement::SetText( const char* inText )\r
+{\r
+       if ( FirstChild() && FirstChild()->ToText() )\r
+               FirstChild()->SetValue( inText );\r
+       else {\r
+               XMLText*        theText = GetDocument()->NewText( inText );\r
+               InsertFirstChild( theText );\r
+       }\r
+}\r
+\r
+\r
+void XMLElement::SetText( int v )\r
+{\r
+    char buf[BUF_SIZE];\r
+    XMLUtil::ToStr( v, buf, BUF_SIZE );\r
+    SetText( buf );\r
+}\r
+\r
+\r
+void XMLElement::SetText( unsigned v )\r
+{\r
+    char buf[BUF_SIZE];\r
+    XMLUtil::ToStr( v, buf, BUF_SIZE );\r
+    SetText( buf );\r
+}\r
+\r
+\r
+void XMLElement::SetText(int64_t v)\r
+{\r
+       char buf[BUF_SIZE];\r
+       XMLUtil::ToStr(v, buf, BUF_SIZE);\r
+       SetText(buf);\r
+}\r
+\r
+\r
+void XMLElement::SetText( bool v )\r
+{\r
+    char buf[BUF_SIZE];\r
+    XMLUtil::ToStr( v, buf, BUF_SIZE );\r
+    SetText( buf );\r
+}\r
+\r
+\r
+void XMLElement::SetText( float v )\r
+{\r
+    char buf[BUF_SIZE];\r
+    XMLUtil::ToStr( v, buf, BUF_SIZE );\r
+    SetText( buf );\r
+}\r
+\r
+\r
+void XMLElement::SetText( double v )\r
+{\r
+    char buf[BUF_SIZE];\r
+    XMLUtil::ToStr( v, buf, BUF_SIZE );\r
+    SetText( buf );\r
+}\r
+\r
+\r
+XMLError XMLElement::QueryIntText( int* ival ) const\r
+{\r
+    if ( FirstChild() && FirstChild()->ToText() ) {\r
+        const char* t = FirstChild()->Value();\r
+        if ( XMLUtil::ToInt( t, ival ) ) {\r
+            return XML_SUCCESS;\r
+        }\r
+        return XML_CAN_NOT_CONVERT_TEXT;\r
+    }\r
+    return XML_NO_TEXT_NODE;\r
+}\r
+\r
+\r
+XMLError XMLElement::QueryUnsignedText( unsigned* uval ) const\r
+{\r
+    if ( FirstChild() && FirstChild()->ToText() ) {\r
+        const char* t = FirstChild()->Value();\r
+        if ( XMLUtil::ToUnsigned( t, uval ) ) {\r
+            return XML_SUCCESS;\r
+        }\r
+        return XML_CAN_NOT_CONVERT_TEXT;\r
+    }\r
+    return XML_NO_TEXT_NODE;\r
+}\r
+\r
+\r
+XMLError XMLElement::QueryInt64Text(int64_t* ival) const\r
+{\r
+       if (FirstChild() && FirstChild()->ToText()) {\r
+               const char* t = FirstChild()->Value();\r
+               if (XMLUtil::ToInt64(t, ival)) {\r
+                       return XML_SUCCESS;\r
+               }\r
+               return XML_CAN_NOT_CONVERT_TEXT;\r
+       }\r
+       return XML_NO_TEXT_NODE;\r
+}\r
+\r
+\r
+XMLError XMLElement::QueryBoolText( bool* bval ) const\r
+{\r
+    if ( FirstChild() && FirstChild()->ToText() ) {\r
+        const char* t = FirstChild()->Value();\r
+        if ( XMLUtil::ToBool( t, bval ) ) {\r
+            return XML_SUCCESS;\r
+        }\r
+        return XML_CAN_NOT_CONVERT_TEXT;\r
+    }\r
+    return XML_NO_TEXT_NODE;\r
+}\r
+\r
+\r
+XMLError XMLElement::QueryDoubleText( double* dval ) const\r
+{\r
+    if ( FirstChild() && FirstChild()->ToText() ) {\r
+        const char* t = FirstChild()->Value();\r
+        if ( XMLUtil::ToDouble( t, dval ) ) {\r
+            return XML_SUCCESS;\r
+        }\r
+        return XML_CAN_NOT_CONVERT_TEXT;\r
+    }\r
+    return XML_NO_TEXT_NODE;\r
+}\r
+\r
+\r
+XMLError XMLElement::QueryFloatText( float* fval ) const\r
+{\r
+    if ( FirstChild() && FirstChild()->ToText() ) {\r
+        const char* t = FirstChild()->Value();\r
+        if ( XMLUtil::ToFloat( t, fval ) ) {\r
+            return XML_SUCCESS;\r
+        }\r
+        return XML_CAN_NOT_CONVERT_TEXT;\r
+    }\r
+    return XML_NO_TEXT_NODE;\r
+}\r
+\r
+int XMLElement::IntText(int defaultValue) const\r
+{\r
+       int i = defaultValue;\r
+       QueryIntText(&i);\r
+       return i;\r
+}\r
+\r
+unsigned XMLElement::UnsignedText(unsigned defaultValue) const\r
+{\r
+       unsigned i = defaultValue;\r
+       QueryUnsignedText(&i);\r
+       return i;\r
+}\r
+\r
+int64_t XMLElement::Int64Text(int64_t defaultValue) const\r
+{\r
+       int64_t i = defaultValue;\r
+       QueryInt64Text(&i);\r
+       return i;\r
+}\r
+\r
+bool XMLElement::BoolText(bool defaultValue) const\r
+{\r
+       bool b = defaultValue;\r
+       QueryBoolText(&b);\r
+       return b;\r
+}\r
+\r
+double XMLElement::DoubleText(double defaultValue) const\r
+{\r
+       double d = defaultValue;\r
+       QueryDoubleText(&d);\r
+       return d;\r
+}\r
+\r
+float XMLElement::FloatText(float defaultValue) const\r
+{\r
+       float f = defaultValue;\r
+       QueryFloatText(&f);\r
+       return f;\r
+}\r
+\r
+\r
+XMLAttribute* XMLElement::FindOrCreateAttribute( const char* name )\r
+{\r
+    XMLAttribute* last = 0;\r
+    XMLAttribute* attrib = 0;\r
+    for( attrib = _rootAttribute;\r
+            attrib;\r
+            last = attrib, attrib = attrib->_next ) {\r
+        if ( XMLUtil::StringEqual( attrib->Name(), name ) ) {\r
+            break;\r
+        }\r
+    }\r
+    if ( !attrib ) {\r
+        attrib = CreateAttribute();\r
+        TIXMLASSERT( attrib );\r
+        if ( last ) {\r
+            TIXMLASSERT( last->_next == 0 );\r
+            last->_next = attrib;\r
+        }\r
+        else {\r
+            TIXMLASSERT( _rootAttribute == 0 );\r
+            _rootAttribute = attrib;\r
+        }\r
+        attrib->SetName( name );\r
+    }\r
+    return attrib;\r
+}\r
+\r
+\r
+void XMLElement::DeleteAttribute( const char* name )\r
+{\r
+    XMLAttribute* prev = 0;\r
+    for( XMLAttribute* a=_rootAttribute; a; a=a->_next ) {\r
+        if ( XMLUtil::StringEqual( name, a->Name() ) ) {\r
+            if ( prev ) {\r
+                prev->_next = a->_next;\r
+            }\r
+            else {\r
+                _rootAttribute = a->_next;\r
+            }\r
+            DeleteAttribute( a );\r
+            break;\r
+        }\r
+        prev = a;\r
+    }\r
+}\r
+\r
+\r
+char* XMLElement::ParseAttributes( char* p, int* curLineNumPtr )\r
+{\r
+    XMLAttribute* prevAttribute = 0;\r
+\r
+    // Read the attributes.\r
+    while( p ) {\r
+        p = XMLUtil::SkipWhiteSpace( p, curLineNumPtr );\r
+        if ( !(*p) ) {\r
+            _document->SetError( XML_ERROR_PARSING_ELEMENT, _parseLineNum, "XMLElement name=%s", Name() );\r
+            return 0;\r
+        }\r
+\r
+        // attribute.\r
+        if (XMLUtil::IsNameStartChar( *p ) ) {\r
+            XMLAttribute* attrib = CreateAttribute();\r
+            TIXMLASSERT( attrib );\r
+            attrib->_parseLineNum = _document->_parseCurLineNum;\r
+\r
+            int attrLineNum = attrib->_parseLineNum;\r
+\r
+            p = attrib->ParseDeep( p, _document->ProcessEntities(), curLineNumPtr );\r
+            if ( !p || Attribute( attrib->Name() ) ) {\r
+                DeleteAttribute( attrib );\r
+                _document->SetError( XML_ERROR_PARSING_ATTRIBUTE, attrLineNum, "XMLElement name=%s", Name() );\r
+                return 0;\r
+            }\r
+            // There is a minor bug here: if the attribute in the source xml\r
+            // document is duplicated, it will not be detected and the\r
+            // attribute will be doubly added. However, tracking the 'prevAttribute'\r
+            // avoids re-scanning the attribute list. Preferring performance for\r
+            // now, may reconsider in the future.\r
+            if ( prevAttribute ) {\r
+                TIXMLASSERT( prevAttribute->_next == 0 );\r
+                prevAttribute->_next = attrib;\r
+            }\r
+            else {\r
+                TIXMLASSERT( _rootAttribute == 0 );\r
+                _rootAttribute = attrib;\r
+            }\r
+            prevAttribute = attrib;\r
+        }\r
+        // end of the tag\r
+        else if ( *p == '>' ) {\r
+            ++p;\r
+            break;\r
+        }\r
+        // end of the tag\r
+        else if ( *p == '/' && *(p+1) == '>' ) {\r
+            _closingType = CLOSED;\r
+            return p+2;        // done; sealed element.\r
+        }\r
+        else {\r
+            _document->SetError( XML_ERROR_PARSING_ELEMENT, _parseLineNum, 0 );\r
+            return 0;\r
+        }\r
+    }\r
+    return p;\r
+}\r
+\r
+void XMLElement::DeleteAttribute( XMLAttribute* attribute )\r
+{\r
+    if ( attribute == 0 ) {\r
+        return;\r
+    }\r
+    MemPool* pool = attribute->_memPool;\r
+    attribute->~XMLAttribute();\r
+    pool->Free( attribute );\r
+}\r
+\r
+XMLAttribute* XMLElement::CreateAttribute()\r
+{\r
+    TIXMLASSERT( sizeof( XMLAttribute ) == _document->_attributePool.ItemSize() );\r
+    XMLAttribute* attrib = new (_document->_attributePool.Alloc() ) XMLAttribute();\r
+    TIXMLASSERT( attrib );\r
+    attrib->_memPool = &_document->_attributePool;\r
+    attrib->_memPool->SetTracked();\r
+    return attrib;\r
+}\r
+\r
+//\r
+//     <ele></ele>\r
+//     <ele>foo<b>bar</b></ele>\r
+//\r
+char* XMLElement::ParseDeep( char* p, StrPair* parentEndTag, int* curLineNumPtr )\r
+{\r
+    // Read the element name.\r
+    p = XMLUtil::SkipWhiteSpace( p, curLineNumPtr );\r
+\r
+    // The closing element is the </element> form. It is\r
+    // parsed just like a regular element then deleted from\r
+    // the DOM.\r
+    if ( *p == '/' ) {\r
+        _closingType = CLOSING;\r
+        ++p;\r
+    }\r
+\r
+    p = _value.ParseName( p );\r
+    if ( _value.Empty() ) {\r
+        return 0;\r
+    }\r
+\r
+    p = ParseAttributes( p, curLineNumPtr );\r
+    if ( !p || !*p || _closingType != OPEN ) {\r
+        return p;\r
+    }\r
+\r
+    p = XMLNode::ParseDeep( p, parentEndTag, curLineNumPtr );\r
+    return p;\r
+}\r
+\r
+\r
+\r
+XMLNode* XMLElement::ShallowClone( XMLDocument* doc ) const\r
+{\r
+    if ( !doc ) {\r
+        doc = _document;\r
+    }\r
+    XMLElement* element = doc->NewElement( Value() );                                  // fixme: this will always allocate memory. Intern?\r
+    for( const XMLAttribute* a=FirstAttribute(); a; a=a->Next() ) {\r
+        element->SetAttribute( a->Name(), a->Value() );                                        // fixme: this will always allocate memory. Intern?\r
+    }\r
+    return element;\r
+}\r
+\r
+\r
+bool XMLElement::ShallowEqual( const XMLNode* compare ) const\r
+{\r
+    TIXMLASSERT( compare );\r
+    const XMLElement* other = compare->ToElement();\r
+    if ( other && XMLUtil::StringEqual( other->Name(), Name() )) {\r
+\r
+        const XMLAttribute* a=FirstAttribute();\r
+        const XMLAttribute* b=other->FirstAttribute();\r
+\r
+        while ( a && b ) {\r
+            if ( !XMLUtil::StringEqual( a->Value(), b->Value() ) ) {\r
+                return false;\r
+            }\r
+            a = a->Next();\r
+            b = b->Next();\r
+        }\r
+        if ( a || b ) {\r
+            // different count\r
+            return false;\r
+        }\r
+        return true;\r
+    }\r
+    return false;\r
+}\r
+\r
+\r
+bool XMLElement::Accept( XMLVisitor* visitor ) const\r
+{\r
+    TIXMLASSERT( visitor );\r
+    if ( visitor->VisitEnter( *this, _rootAttribute ) ) {\r
+        for ( const XMLNode* node=FirstChild(); node; node=node->NextSibling() ) {\r
+            if ( !node->Accept( visitor ) ) {\r
+                break;\r
+            }\r
+        }\r
+    }\r
+    return visitor->VisitExit( *this );\r
+}\r
+\r
+\r
+// --------- XMLDocument ----------- //\r
+\r
+// Warning: List must match 'enum XMLError'\r
+const char* XMLDocument::_errorNames[XML_ERROR_COUNT] = {\r
+    "XML_SUCCESS",\r
+    "XML_NO_ATTRIBUTE",\r
+    "XML_WRONG_ATTRIBUTE_TYPE",\r
+    "XML_ERROR_FILE_NOT_FOUND",\r
+    "XML_ERROR_FILE_COULD_NOT_BE_OPENED",\r
+    "XML_ERROR_FILE_READ_ERROR",\r
+    "XML_ERROR_PARSING_ELEMENT",\r
+    "XML_ERROR_PARSING_ATTRIBUTE",\r
+    "XML_ERROR_PARSING_TEXT",\r
+    "XML_ERROR_PARSING_CDATA",\r
+    "XML_ERROR_PARSING_COMMENT",\r
+    "XML_ERROR_PARSING_DECLARATION",\r
+    "XML_ERROR_PARSING_UNKNOWN",\r
+    "XML_ERROR_EMPTY_DOCUMENT",\r
+    "XML_ERROR_MISMATCHED_ELEMENT",\r
+    "XML_ERROR_PARSING",\r
+    "XML_CAN_NOT_CONVERT_TEXT",\r
+    "XML_NO_TEXT_NODE",\r
+       "XML_ELEMENT_DEPTH_EXCEEDED"\r
+};\r
+\r
+\r
+XMLDocument::XMLDocument( bool processEntities, Whitespace whitespaceMode ) :\r
+    XMLNode( 0 ),\r
+    _writeBOM( false ),\r
+    _processEntities( processEntities ),\r
+    _errorID(XML_SUCCESS),\r
+    _whitespaceMode( whitespaceMode ),\r
+    _errorStr(),\r
+    _errorLineNum( 0 ),\r
+    _charBuffer( 0 ),\r
+    _parseCurLineNum( 0 ),\r
+       _parsingDepth(0),\r
+    _unlinked(),\r
+    _elementPool(),\r
+    _attributePool(),\r
+    _textPool(),\r
+    _commentPool()\r
+{\r
+    // avoid VC++ C4355 warning about 'this' in initializer list (C4355 is off by default in VS2012+)\r
+    _document = this;\r
+}\r
+\r
+\r
+XMLDocument::~XMLDocument()\r
+{\r
+    Clear();\r
+}\r
+\r
+\r
+void XMLDocument::MarkInUse(XMLNode* node)\r
+{\r
+       TIXMLASSERT(node);\r
+       TIXMLASSERT(node->_parent == 0);\r
+\r
+       for (int i = 0; i < _unlinked.Size(); ++i) {\r
+               if (node == _unlinked[i]) {\r
+                       _unlinked.SwapRemove(i);\r
+                       break;\r
+               }\r
+       }\r
+}\r
+\r
+void XMLDocument::Clear()\r
+{\r
+    DeleteChildren();\r
+       while( _unlinked.Size()) {\r
+               DeleteNode(_unlinked[0]);       // Will remove from _unlinked as part of delete.\r
+       }\r
+\r
+#ifdef TINYXML2_DEBUG\r
+    const bool hadError = Error();\r
+#endif\r
+    ClearError();\r
+\r
+    delete [] _charBuffer;\r
+    _charBuffer = 0;\r
+       _parsingDepth = 0;\r
+\r
+#if 0\r
+    _textPool.Trace( "text" );\r
+    _elementPool.Trace( "element" );\r
+    _commentPool.Trace( "comment" );\r
+    _attributePool.Trace( "attribute" );\r
+#endif\r
+\r
+#ifdef TINYXML2_DEBUG\r
+    if ( !hadError ) {\r
+        TIXMLASSERT( _elementPool.CurrentAllocs()   == _elementPool.Untracked() );\r
+        TIXMLASSERT( _attributePool.CurrentAllocs() == _attributePool.Untracked() );\r
+        TIXMLASSERT( _textPool.CurrentAllocs()      == _textPool.Untracked() );\r
+        TIXMLASSERT( _commentPool.CurrentAllocs()   == _commentPool.Untracked() );\r
+    }\r
+#endif\r
+}\r
+\r
+\r
+void XMLDocument::DeepCopy(XMLDocument* target) const\r
+{\r
+       TIXMLASSERT(target);\r
+    if (target == this) {\r
+        return; // technically success - a no-op.\r
+    }\r
+\r
+       target->Clear();\r
+       for (const XMLNode* node = this->FirstChild(); node; node = node->NextSibling()) {\r
+               target->InsertEndChild(node->DeepClone(target));\r
+       }\r
+}\r
+\r
+XMLElement* XMLDocument::NewElement( const char* name )\r
+{\r
+    XMLElement* ele = CreateUnlinkedNode<XMLElement>( _elementPool );\r
+    ele->SetName( name );\r
+    return ele;\r
+}\r
+\r
+\r
+XMLComment* XMLDocument::NewComment( const char* str )\r
+{\r
+    XMLComment* comment = CreateUnlinkedNode<XMLComment>( _commentPool );\r
+    comment->SetValue( str );\r
+    return comment;\r
+}\r
+\r
+\r
+XMLText* XMLDocument::NewText( const char* str )\r
+{\r
+    XMLText* text = CreateUnlinkedNode<XMLText>( _textPool );\r
+    text->SetValue( str );\r
+    return text;\r
+}\r
+\r
+\r
+XMLDeclaration* XMLDocument::NewDeclaration( const char* str )\r
+{\r
+    XMLDeclaration* dec = CreateUnlinkedNode<XMLDeclaration>( _commentPool );\r
+    dec->SetValue( str ? str : "xml version=\"1.0\" encoding=\"UTF-8\"" );\r
+    return dec;\r
+}\r
+\r
+\r
+XMLUnknown* XMLDocument::NewUnknown( const char* str )\r
+{\r
+    XMLUnknown* unk = CreateUnlinkedNode<XMLUnknown>( _commentPool );\r
+    unk->SetValue( str );\r
+    return unk;\r
+}\r
+\r
+static FILE* callfopen( const char* filepath, const char* mode )\r
+{\r
+    TIXMLASSERT( filepath );\r
+    TIXMLASSERT( mode );\r
+#if defined(_MSC_VER) && (_MSC_VER >= 1400 ) && (!defined WINCE)\r
+    FILE* fp = 0;\r
+    errno_t err = fopen_s( &fp, filepath, mode );\r
+    if ( err ) {\r
+        return 0;\r
+    }\r
+#else\r
+    FILE* fp = fopen( filepath, mode );\r
+#endif\r
+    return fp;\r
+}\r
+\r
+void XMLDocument::DeleteNode( XMLNode* node )  {\r
+    TIXMLASSERT( node );\r
+    TIXMLASSERT(node->_document == this );\r
+    if (node->_parent) {\r
+        node->_parent->DeleteChild( node );\r
+    }\r
+    else {\r
+        // Isn't in the tree.\r
+        // Use the parent delete.\r
+        // Also, we need to mark it tracked: we 'know'\r
+        // it was never used.\r
+        node->_memPool->SetTracked();\r
+        // Call the static XMLNode version:\r
+        XMLNode::DeleteNode(node);\r
+    }\r
+}\r
+\r
+\r
+XMLError XMLDocument::LoadFile( const char* filename )\r
+{\r
+    if ( !filename ) {\r
+        TIXMLASSERT( false );\r
+        SetError( XML_ERROR_FILE_COULD_NOT_BE_OPENED, 0, "filename=<null>" );\r
+        return _errorID;\r
+    }\r
+\r
+    Clear();\r
+    FILE* fp = callfopen( filename, "rb" );\r
+    if ( !fp ) {\r
+        SetError( XML_ERROR_FILE_NOT_FOUND, 0, "filename=%s", filename );\r
+        return _errorID;\r
+    }\r
+    LoadFile( fp );\r
+    fclose( fp );\r
+    return _errorID;\r
+}\r
+\r
+// This is likely overengineered template art to have a check that unsigned long value incremented\r
+// by one still fits into size_t. If size_t type is larger than unsigned long type\r
+// (x86_64-w64-mingw32 target) then the check is redundant and gcc and clang emit\r
+// -Wtype-limits warning. This piece makes the compiler select code with a check when a check\r
+// is useful and code with no check when a check is redundant depending on how size_t and unsigned long\r
+// types sizes relate to each other.\r
+template\r
+<bool = (sizeof(unsigned long) >= sizeof(size_t))>\r
+struct LongFitsIntoSizeTMinusOne {\r
+    static bool Fits( unsigned long value )\r
+    {\r
+        return value < (size_t)-1;\r
+    }\r
+};\r
+\r
+template <>\r
+struct LongFitsIntoSizeTMinusOne<false> {\r
+    static bool Fits( unsigned long )\r
+    {\r
+        return true;\r
+    }\r
+};\r
+\r
+XMLError XMLDocument::LoadFile( FILE* fp )\r
+{\r
+    Clear();\r
+\r
+    fseek( fp, 0, SEEK_SET );\r
+    if ( fgetc( fp ) == EOF && ferror( fp ) != 0 ) {\r
+        SetError( XML_ERROR_FILE_READ_ERROR, 0, 0 );\r
+        return _errorID;\r
+    }\r
+\r
+    fseek( fp, 0, SEEK_END );\r
+    const long filelength = ftell( fp );\r
+    fseek( fp, 0, SEEK_SET );\r
+    if ( filelength == -1L ) {\r
+        SetError( XML_ERROR_FILE_READ_ERROR, 0, 0 );\r
+        return _errorID;\r
+    }\r
+    TIXMLASSERT( filelength >= 0 );\r
+\r
+    if ( !LongFitsIntoSizeTMinusOne<>::Fits( filelength ) ) {\r
+        // Cannot handle files which won't fit in buffer together with null terminator\r
+        SetError( XML_ERROR_FILE_READ_ERROR, 0, 0 );\r
+        return _errorID;\r
+    }\r
+\r
+    if ( filelength == 0 ) {\r
+        SetError( XML_ERROR_EMPTY_DOCUMENT, 0, 0 );\r
+        return _errorID;\r
+    }\r
+\r
+    const size_t size = filelength;\r
+    TIXMLASSERT( _charBuffer == 0 );\r
+    _charBuffer = new char[size+1];\r
+    size_t read = fread( _charBuffer, 1, size, fp );\r
+    if ( read != size ) {\r
+        SetError( XML_ERROR_FILE_READ_ERROR, 0, 0 );\r
+        return _errorID;\r
+    }\r
+\r
+    _charBuffer[size] = 0;\r
+\r
+    Parse();\r
+    return _errorID;\r
+}\r
+\r
+\r
+XMLError XMLDocument::SaveFile( const char* filename, bool compact )\r
+{\r
+    if ( !filename ) {\r
+        TIXMLASSERT( false );\r
+        SetError( XML_ERROR_FILE_COULD_NOT_BE_OPENED, 0, "filename=<null>" );\r
+        return _errorID;\r
+    }\r
+\r
+    FILE* fp = callfopen( filename, "w" );\r
+    if ( !fp ) {\r
+        SetError( XML_ERROR_FILE_COULD_NOT_BE_OPENED, 0, "filename=%s", filename );\r
+        return _errorID;\r
+    }\r
+    SaveFile(fp, compact);\r
+    fclose( fp );\r
+    return _errorID;\r
+}\r
+\r
+\r
+XMLError XMLDocument::SaveFile( FILE* fp, bool compact )\r
+{\r
+    // Clear any error from the last save, otherwise it will get reported\r
+    // for *this* call.\r
+    ClearError();\r
+    XMLPrinter stream( fp, compact );\r
+    Print( &stream );\r
+    return _errorID;\r
+}\r
+\r
+\r
+XMLError XMLDocument::Parse( const char* p, size_t len )\r
+{\r
+    Clear();\r
+\r
+    if ( len == 0 || !p || !*p ) {\r
+        SetError( XML_ERROR_EMPTY_DOCUMENT, 0, 0 );\r
+        return _errorID;\r
+    }\r
+    if ( len == (size_t)(-1) ) {\r
+        len = strlen( p );\r
+    }\r
+    TIXMLASSERT( _charBuffer == 0 );\r
+    _charBuffer = new char[ len+1 ];\r
+    memcpy( _charBuffer, p, len );\r
+    _charBuffer[len] = 0;\r
+\r
+    Parse();\r
+    if ( Error() ) {\r
+        // clean up now essentially dangling memory.\r
+        // and the parse fail can put objects in the\r
+        // pools that are dead and inaccessible.\r
+        DeleteChildren();\r
+        _elementPool.Clear();\r
+        _attributePool.Clear();\r
+        _textPool.Clear();\r
+        _commentPool.Clear();\r
+    }\r
+    return _errorID;\r
+}\r
+\r
+\r
+void XMLDocument::Print( XMLPrinter* streamer ) const\r
+{\r
+    if ( streamer ) {\r
+        Accept( streamer );\r
+    }\r
+    else {\r
+        XMLPrinter stdoutStreamer( stdout );\r
+        Accept( &stdoutStreamer );\r
+    }\r
+}\r
+\r
+\r
+void XMLDocument::SetError( XMLError error, int lineNum, const char* format, ... )\r
+{\r
+    TIXMLASSERT( error >= 0 && error < XML_ERROR_COUNT );\r
+    _errorID = error;\r
+    _errorLineNum = lineNum;\r
+       _errorStr.Reset();\r
+\r
+    size_t BUFFER_SIZE = 1000;\r
+    char* buffer = new char[BUFFER_SIZE];\r
+\r
+    TIXMLASSERT(sizeof(error) <= sizeof(int));\r
+    TIXML_SNPRINTF(buffer, BUFFER_SIZE, "Error=%s ErrorID=%d (0x%x) Line number=%d", ErrorIDToName(error), int(error), int(error), lineNum);\r
+\r
+       if (format) {\r
+               size_t len = strlen(buffer);\r
+               TIXML_SNPRINTF(buffer + len, BUFFER_SIZE - len, ": ");\r
+               len = strlen(buffer);\r
+\r
+               va_list va;\r
+               va_start(va, format);\r
+               TIXML_VSNPRINTF(buffer + len, BUFFER_SIZE - len, format, va);\r
+               va_end(va);\r
+       }\r
+       _errorStr.SetStr(buffer);\r
+       delete[] buffer;\r
+}\r
+\r
+\r
+/*static*/ const char* XMLDocument::ErrorIDToName(XMLError errorID)\r
+{\r
+       TIXMLASSERT( errorID >= 0 && errorID < XML_ERROR_COUNT );\r
+    const char* errorName = _errorNames[errorID];\r
+    TIXMLASSERT( errorName && errorName[0] );\r
+    return errorName;\r
+}\r
+\r
+const char* XMLDocument::ErrorStr() const\r
+{\r
+       return _errorStr.Empty() ? "" : _errorStr.GetStr();\r
+}\r
+\r
+\r
+void XMLDocument::PrintError() const\r
+{\r
+    printf("%s\n", ErrorStr());\r
+}\r
+\r
+const char* XMLDocument::ErrorName() const\r
+{\r
+    return ErrorIDToName(_errorID);\r
+}\r
+\r
+void XMLDocument::Parse()\r
+{\r
+    TIXMLASSERT( NoChildren() ); // Clear() must have been called previously\r
+    TIXMLASSERT( _charBuffer );\r
+    _parseCurLineNum = 1;\r
+    _parseLineNum = 1;\r
+    char* p = _charBuffer;\r
+    p = XMLUtil::SkipWhiteSpace( p, &_parseCurLineNum );\r
+    p = const_cast<char*>( XMLUtil::ReadBOM( p, &_writeBOM ) );\r
+    if ( !*p ) {\r
+        SetError( XML_ERROR_EMPTY_DOCUMENT, 0, 0 );\r
+        return;\r
+    }\r
+    ParseDeep(p, 0, &_parseCurLineNum );\r
+}\r
+\r
+void XMLDocument::PushDepth()\r
+{\r
+       _parsingDepth++;\r
+       if (_parsingDepth == TINYXML2_MAX_ELEMENT_DEPTH) {\r
+               SetError(XML_ELEMENT_DEPTH_EXCEEDED, _parseCurLineNum, "Element nesting is too deep." );\r
+       }\r
+}\r
+\r
+void XMLDocument::PopDepth()\r
+{\r
+       TIXMLASSERT(_parsingDepth > 0);\r
+       --_parsingDepth;\r
+}\r
+\r
+XMLPrinter::XMLPrinter( FILE* file, bool compact, int depth ) :\r
+    _elementJustOpened( false ),\r
+    _stack(),\r
+    _firstElement( true ),\r
+    _fp( file ),\r
+    _depth( depth ),\r
+    _textDepth( -1 ),\r
+    _processEntities( true ),\r
+    _compactMode( compact ),\r
+    _buffer()\r
+{\r
+    for( int i=0; i<ENTITY_RANGE; ++i ) {\r
+        _entityFlag[i] = false;\r
+        _restrictedEntityFlag[i] = false;\r
+    }\r
+    for( int i=0; i<NUM_ENTITIES; ++i ) {\r
+        const char entityValue = entities[i].value;\r
+        const unsigned char flagIndex = (unsigned char)entityValue;\r
+        TIXMLASSERT( flagIndex < ENTITY_RANGE );\r
+        _entityFlag[flagIndex] = true;\r
+    }\r
+    _restrictedEntityFlag[(unsigned char)'&'] = true;\r
+    _restrictedEntityFlag[(unsigned char)'<'] = true;\r
+    _restrictedEntityFlag[(unsigned char)'>'] = true;  // not required, but consistency is nice\r
+    _buffer.Push( 0 );\r
+}\r
+\r
+\r
+void XMLPrinter::Print( const char* format, ... )\r
+{\r
+    va_list     va;\r
+    va_start( va, format );\r
+\r
+    if ( _fp ) {\r
+        vfprintf( _fp, format, va );\r
+    }\r
+    else {\r
+        const int len = TIXML_VSCPRINTF( format, va );\r
+        // Close out and re-start the va-args\r
+        va_end( va );\r
+        TIXMLASSERT( len >= 0 );\r
+        va_start( va, format );\r
+        TIXMLASSERT( _buffer.Size() > 0 && _buffer[_buffer.Size() - 1] == 0 );\r
+        char* p = _buffer.PushArr( len ) - 1;  // back up over the null terminator.\r
+               TIXML_VSNPRINTF( p, len+1, format, va );\r
+    }\r
+    va_end( va );\r
+}\r
+\r
+\r
+void XMLPrinter::Write( const char* data, size_t size )\r
+{\r
+    if ( _fp ) {\r
+        fwrite ( data , sizeof(char), size, _fp);\r
+    }\r
+    else {\r
+        char* p = _buffer.PushArr( static_cast<int>(size) ) - 1;   // back up over the null terminator.\r
+        memcpy( p, data, size );\r
+        p[size] = 0;\r
+    }\r
+}\r
+\r
+\r
+void XMLPrinter::Putc( char ch )\r
+{\r
+    if ( _fp ) {\r
+        fputc ( ch, _fp);\r
+    }\r
+    else {\r
+        char* p = _buffer.PushArr( sizeof(char) ) - 1;   // back up over the null terminator.\r
+        p[0] = ch;\r
+        p[1] = 0;\r
+    }\r
+}\r
+\r
+\r
+void XMLPrinter::PrintSpace( int depth )\r
+{\r
+    for( int i=0; i<depth; ++i ) {\r
+        Write( "    " );\r
+    }\r
+}\r
+\r
+\r
+void XMLPrinter::PrintString( const char* p, bool restricted )\r
+{\r
+    // Look for runs of bytes between entities to print.\r
+    const char* q = p;\r
+\r
+    if ( _processEntities ) {\r
+        const bool* flag = restricted ? _restrictedEntityFlag : _entityFlag;\r
+        while ( *q ) {\r
+            TIXMLASSERT( p <= q );\r
+            // Remember, char is sometimes signed. (How many times has that bitten me?)\r
+            if ( *q > 0 && *q < ENTITY_RANGE ) {\r
+                // Check for entities. If one is found, flush\r
+                // the stream up until the entity, write the\r
+                // entity, and keep looking.\r
+                if ( flag[(unsigned char)(*q)] ) {\r
+                    while ( p < q ) {\r
+                        const size_t delta = q - p;\r
+                        const int toPrint = ( INT_MAX < delta ) ? INT_MAX : (int)delta;\r
+                        Write( p, toPrint );\r
+                        p += toPrint;\r
+                    }\r
+                    bool entityPatternPrinted = false;\r
+                    for( int i=0; i<NUM_ENTITIES; ++i ) {\r
+                        if ( entities[i].value == *q ) {\r
+                            Putc( '&' );\r
+                            Write( entities[i].pattern, entities[i].length );\r
+                            Putc( ';' );\r
+                            entityPatternPrinted = true;\r
+                            break;\r
+                        }\r
+                    }\r
+                    if ( !entityPatternPrinted ) {\r
+                        // TIXMLASSERT( entityPatternPrinted ) causes gcc -Wunused-but-set-variable in release\r
+                        TIXMLASSERT( false );\r
+                    }\r
+                    ++p;\r
+                }\r
+            }\r
+            ++q;\r
+            TIXMLASSERT( p <= q );\r
+        }\r
+        // Flush the remaining string. This will be the entire\r
+        // string if an entity wasn't found.\r
+        if ( p < q ) {\r
+            const size_t delta = q - p;\r
+            const int toPrint = ( INT_MAX < delta ) ? INT_MAX : (int)delta;\r
+            Write( p, toPrint );\r
+        }\r
+    }\r
+    else {\r
+        Write( p );\r
+    }\r
+}\r
+\r
+\r
+void XMLPrinter::PushHeader( bool writeBOM, bool writeDec )\r
+{\r
+    if ( writeBOM ) {\r
+        static const unsigned char bom[] = { TIXML_UTF_LEAD_0, TIXML_UTF_LEAD_1, TIXML_UTF_LEAD_2, 0 };\r
+        Write( reinterpret_cast< const char* >( bom ) );\r
+    }\r
+    if ( writeDec ) {\r
+        PushDeclaration( "xml version=\"1.0\"" );\r
+    }\r
+}\r
+\r
+\r
+void XMLPrinter::OpenElement( const char* name, bool compactMode )\r
+{\r
+    SealElementIfJustOpened();\r
+    _stack.Push( name );\r
+\r
+    if ( _textDepth < 0 && !_firstElement && !compactMode ) {\r
+        Putc( '\n' );\r
+    }\r
+    if ( !compactMode ) {\r
+        PrintSpace( _depth );\r
+    }\r
+\r
+    Write ( "<" );\r
+    Write ( name );\r
+\r
+    _elementJustOpened = true;\r
+    _firstElement = false;\r
+    ++_depth;\r
+}\r
+\r
+\r
+void XMLPrinter::PushAttribute( const char* name, const char* value )\r
+{\r
+    TIXMLASSERT( _elementJustOpened );\r
+    Putc ( ' ' );\r
+    Write( name );\r
+    Write( "=\"" );\r
+    PrintString( value, false );\r
+    Putc ( '\"' );\r
+}\r
+\r
+\r
+void XMLPrinter::PushAttribute( const char* name, int v )\r
+{\r
+    char buf[BUF_SIZE];\r
+    XMLUtil::ToStr( v, buf, BUF_SIZE );\r
+    PushAttribute( name, buf );\r
+}\r
+\r
+\r
+void XMLPrinter::PushAttribute( const char* name, unsigned v )\r
+{\r
+    char buf[BUF_SIZE];\r
+    XMLUtil::ToStr( v, buf, BUF_SIZE );\r
+    PushAttribute( name, buf );\r
+}\r
+\r
+\r
+void XMLPrinter::PushAttribute(const char* name, int64_t v)\r
+{\r
+       char buf[BUF_SIZE];\r
+       XMLUtil::ToStr(v, buf, BUF_SIZE);\r
+       PushAttribute(name, buf);\r
+}\r
+\r
+\r
+void XMLPrinter::PushAttribute( const char* name, bool v )\r
+{\r
+    char buf[BUF_SIZE];\r
+    XMLUtil::ToStr( v, buf, BUF_SIZE );\r
+    PushAttribute( name, buf );\r
+}\r
+\r
+\r
+void XMLPrinter::PushAttribute( const char* name, double v )\r
+{\r
+    char buf[BUF_SIZE];\r
+    XMLUtil::ToStr( v, buf, BUF_SIZE );\r
+    PushAttribute( name, buf );\r
+}\r
+\r
+\r
+void XMLPrinter::CloseElement( bool compactMode )\r
+{\r
+    --_depth;\r
+    const char* name = _stack.Pop();\r
+\r
+    if ( _elementJustOpened ) {\r
+        Write( "/>" );\r
+    }\r
+    else {\r
+        if ( _textDepth < 0 && !compactMode) {\r
+            Putc( '\n' );\r
+            PrintSpace( _depth );\r
+        }\r
+        Write ( "</" );\r
+        Write ( name );\r
+        Write ( ">" );\r
+    }\r
+\r
+    if ( _textDepth == _depth ) {\r
+        _textDepth = -1;\r
+    }\r
+    if ( _depth == 0 && !compactMode) {\r
+        Putc( '\n' );\r
+    }\r
+    _elementJustOpened = false;\r
+}\r
+\r
+\r
+void XMLPrinter::SealElementIfJustOpened()\r
+{\r
+    if ( !_elementJustOpened ) {\r
+        return;\r
+    }\r
+    _elementJustOpened = false;\r
+    Putc( '>' );\r
+}\r
+\r
+\r
+void XMLPrinter::PushText( const char* text, bool cdata )\r
+{\r
+    _textDepth = _depth-1;\r
+\r
+    SealElementIfJustOpened();\r
+    if ( cdata ) {\r
+        Write( "<![CDATA[" );\r
+        Write( text );\r
+        Write( "]]>" );\r
+    }\r
+    else {\r
+        PrintString( text, true );\r
+    }\r
+}\r
+\r
+void XMLPrinter::PushText( int64_t value )\r
+{\r
+    char buf[BUF_SIZE];\r
+    XMLUtil::ToStr( value, buf, BUF_SIZE );\r
+    PushText( buf, false );\r
+}\r
+\r
+void XMLPrinter::PushText( int value )\r
+{\r
+    char buf[BUF_SIZE];\r
+    XMLUtil::ToStr( value, buf, BUF_SIZE );\r
+    PushText( buf, false );\r
+}\r
+\r
+\r
+void XMLPrinter::PushText( unsigned value )\r
+{\r
+    char buf[BUF_SIZE];\r
+    XMLUtil::ToStr( value, buf, BUF_SIZE );\r
+    PushText( buf, false );\r
+}\r
+\r
+\r
+void XMLPrinter::PushText( bool value )\r
+{\r
+    char buf[BUF_SIZE];\r
+    XMLUtil::ToStr( value, buf, BUF_SIZE );\r
+    PushText( buf, false );\r
+}\r
+\r
+\r
+void XMLPrinter::PushText( float value )\r
+{\r
+    char buf[BUF_SIZE];\r
+    XMLUtil::ToStr( value, buf, BUF_SIZE );\r
+    PushText( buf, false );\r
+}\r
+\r
+\r
+void XMLPrinter::PushText( double value )\r
+{\r
+    char buf[BUF_SIZE];\r
+    XMLUtil::ToStr( value, buf, BUF_SIZE );\r
+    PushText( buf, false );\r
+}\r
+\r
+\r
+void XMLPrinter::PushComment( const char* comment )\r
+{\r
+    SealElementIfJustOpened();\r
+    if ( _textDepth < 0 && !_firstElement && !_compactMode) {\r
+        Putc( '\n' );\r
+        PrintSpace( _depth );\r
+    }\r
+    _firstElement = false;\r
+\r
+    Write( "<!--" );\r
+    Write( comment );\r
+    Write( "-->" );\r
+}\r
+\r
+\r
+void XMLPrinter::PushDeclaration( const char* value )\r
+{\r
+    SealElementIfJustOpened();\r
+    if ( _textDepth < 0 && !_firstElement && !_compactMode) {\r
+        Putc( '\n' );\r
+        PrintSpace( _depth );\r
+    }\r
+    _firstElement = false;\r
+\r
+    Write( "<?" );\r
+    Write( value );\r
+    Write( "?>" );\r
+}\r
+\r
+\r
+void XMLPrinter::PushUnknown( const char* value )\r
+{\r
+    SealElementIfJustOpened();\r
+    if ( _textDepth < 0 && !_firstElement && !_compactMode) {\r
+        Putc( '\n' );\r
+        PrintSpace( _depth );\r
+    }\r
+    _firstElement = false;\r
+\r
+    Write( "<!" );\r
+    Write( value );\r
+    Putc( '>' );\r
+}\r
+\r
+\r
+bool XMLPrinter::VisitEnter( const XMLDocument& doc )\r
+{\r
+    _processEntities = doc.ProcessEntities();\r
+    if ( doc.HasBOM() ) {\r
+        PushHeader( true, false );\r
+    }\r
+    return true;\r
+}\r
+\r
+\r
+bool XMLPrinter::VisitEnter( const XMLElement& element, const XMLAttribute* attribute )\r
+{\r
+    const XMLElement* parentElem = 0;\r
+    if ( element.Parent() ) {\r
+        parentElem = element.Parent()->ToElement();\r
+    }\r
+    const bool compactMode = parentElem ? CompactMode( *parentElem ) : _compactMode;\r
+    OpenElement( element.Name(), compactMode );\r
+    while ( attribute ) {\r
+        PushAttribute( attribute->Name(), attribute->Value() );\r
+        attribute = attribute->Next();\r
+    }\r
+    return true;\r
+}\r
+\r
+\r
+bool XMLPrinter::VisitExit( const XMLElement& element )\r
+{\r
+    CloseElement( CompactMode(element) );\r
+    return true;\r
+}\r
+\r
+\r
+bool XMLPrinter::Visit( const XMLText& text )\r
+{\r
+    PushText( text.Value(), text.CData() );\r
+    return true;\r
+}\r
+\r
+\r
+bool XMLPrinter::Visit( const XMLComment& comment )\r
+{\r
+    PushComment( comment.Value() );\r
+    return true;\r
+}\r
+\r
+bool XMLPrinter::Visit( const XMLDeclaration& declaration )\r
+{\r
+    PushDeclaration( declaration.Value() );\r
+    return true;\r
+}\r
+\r
+\r
+bool XMLPrinter::Visit( const XMLUnknown& unknown )\r
+{\r
+    PushUnknown( unknown.Value() );\r
+    return true;\r
+}\r
+\r
+}   // namespace tinyxml2\r
diff --git a/src/private/tinyxml2.h b/src/private/tinyxml2.h
new file mode 100755 (executable)
index 0000000..4d1fec4
--- /dev/null
@@ -0,0 +1,2309 @@
+/*\r
+Original code by Lee Thomason (www.grinninglizard.com)\r
+\r
+This software is provided 'as-is', without any express or implied\r
+warranty. In no event will the authors be held liable for any\r
+damages arising from the use of this software.\r
+\r
+Permission is granted to anyone to use this software for any\r
+purpose, including commercial applications, and to alter it and\r
+redistribute it freely, subject to the following restrictions:\r
+\r
+1. The origin of this software must not be misrepresented; you must\r
+not claim that you wrote the original software. If you use this\r
+software in a product, an acknowledgment in the product documentation\r
+would be appreciated but is not required.\r
+\r
+2. Altered source versions must be plainly marked as such, and\r
+must not be misrepresented as being the original software.\r
+\r
+3. This notice may not be removed or altered from any source\r
+distribution.\r
+*/\r
+\r
+#ifndef BT_TINYXML2_INCLUDED\r
+#define BT_TINYXML2_INCLUDED\r
+\r
+#if defined(ANDROID_NDK) || defined(__BORLANDC__) || defined(__QNXNTO__)\r
+#   include <ctype.h>\r
+#   include <limits.h>\r
+#   include <stdio.h>\r
+#   include <stdlib.h>\r
+#   include <string.h>\r
+#      if defined(__PS3__)\r
+#              include <stddef.h>\r
+#      endif\r
+#else\r
+#   include <cctype>\r
+#   include <climits>\r
+#   include <cstdio>\r
+#   include <cstdlib>\r
+#   include <cstring>\r
+#endif\r
+#include <stdint.h>\r
+\r
+/*\r
+   TODO: intern strings instead of allocation.\r
+*/\r
+/*\r
+       gcc:\r
+        g++ -Wall -DTINYXML2_DEBUG tinyxml2.cpp xmltest.cpp -o gccxmltest.exe\r
+\r
+    Formatting, Artistic Style:\r
+        AStyle.exe --style=1tbs --indent-switches --break-closing-brackets --indent-preprocessor tinyxml2.cpp tinyxml2.h\r
+*/\r
+\r
+#if defined( _DEBUG ) || defined (__DEBUG__)\r
+#   ifndef TINYXML2_DEBUG\r
+#       define TINYXML2_DEBUG\r
+#   endif\r
+#endif\r
+\r
+#ifdef _MSC_VER\r
+#   pragma warning(push)\r
+#   pragma warning(disable: 4251)\r
+#endif\r
+\r
+#ifdef _WIN32\r
+#   ifdef TINYXML2_EXPORT\r
+#       define TINYXML2_LIB __declspec(dllexport)\r
+#   elif defined(TINYXML2_IMPORT)\r
+#       define TINYXML2_LIB __declspec(dllimport)\r
+#   else\r
+#       define TINYXML2_LIB\r
+#   endif\r
+#elif __GNUC__ >= 4\r
+#   define TINYXML2_LIB __attribute__((visibility("hidden")))\r
+#else\r
+#   define TINYXML2_LIB\r
+#endif\r
+\r
+\r
+#if defined(TINYXML2_DEBUG)\r
+#   if defined(_MSC_VER)\r
+#       // "(void)0," is for suppressing C4127 warning in "assert(false)", "assert(true)" and the like\r
+#       define TIXMLASSERT( x )           if ( !((void)0,(x))) { __debugbreak(); }\r
+#   elif defined (ANDROID_NDK)\r
+#       include <android/log.h>\r
+#       define TIXMLASSERT( x )           if ( !(x)) { __android_log_assert( "assert", "grinliz", "ASSERT in '%s' at %d.", __FILE__, __LINE__ ); }\r
+#   else\r
+#       include <assert.h>\r
+#       define TIXMLASSERT                assert\r
+#   endif\r
+#else\r
+#   define TIXMLASSERT( x )               {}\r
+#endif\r
+\r
+\r
+/* Versioning, past 1.0.14:\r
+       http://semver.org/\r
+*/\r
+static const int TIXML2_MAJOR_VERSION = 7;\r
+static const int TIXML2_MINOR_VERSION = 0;\r
+static const int TIXML2_PATCH_VERSION = 1;\r
+\r
+#define TINYXML2_MAJOR_VERSION 7\r
+#define TINYXML2_MINOR_VERSION 0\r
+#define TINYXML2_PATCH_VERSION 1\r
+\r
+// A fixed element depth limit is problematic. There needs to be a\r
+// limit to avoid a stack overflow. However, that limit varies per\r
+// system, and the capacity of the stack. On the other hand, it's a trivial\r
+// attack that can result from ill, malicious, or even correctly formed XML,\r
+// so there needs to be a limit in place.\r
+static const int TINYXML2_MAX_ELEMENT_DEPTH = 100;\r
+\r
+namespace BT_TinyXML2\r
+{\r
+class XMLDocument;\r
+class XMLElement;\r
+class XMLAttribute;\r
+class XMLComment;\r
+class XMLText;\r
+class XMLDeclaration;\r
+class XMLUnknown;\r
+class XMLPrinter;\r
+\r
+/*\r
+       A class that wraps strings. Normally stores the start and end\r
+       pointers into the XML file itself, and will apply normalization\r
+       and entity translation if actually read. Can also store (and memory\r
+       manage) a traditional char[]\r
+\r
+    Isn't clear why TINYXML2_LIB is needed; but seems to fix #719\r
+*/\r
+class TINYXML2_LIB StrPair\r
+{\r
+public:\r
+    enum {\r
+        NEEDS_ENTITY_PROCESSING                        = 0x01,\r
+        NEEDS_NEWLINE_NORMALIZATION            = 0x02,\r
+        NEEDS_WHITESPACE_COLLAPSING     = 0x04,\r
+\r
+        TEXT_ELEMENT                       = NEEDS_ENTITY_PROCESSING | NEEDS_NEWLINE_NORMALIZATION,\r
+        TEXT_ELEMENT_LEAVE_ENTITIES            = NEEDS_NEWLINE_NORMALIZATION,\r
+        ATTRIBUTE_NAME                     = 0,\r
+        ATTRIBUTE_VALUE                            = NEEDS_ENTITY_PROCESSING | NEEDS_NEWLINE_NORMALIZATION,\r
+        ATTRIBUTE_VALUE_LEAVE_ENTITIES  = NEEDS_NEWLINE_NORMALIZATION,\r
+        COMMENT                                                        = NEEDS_NEWLINE_NORMALIZATION\r
+    };\r
+\r
+    StrPair() : _flags( 0 ), _start( 0 ), _end( 0 ) {}\r
+    ~StrPair();\r
+\r
+    void Set( char* start, char* end, int flags ) {\r
+        TIXMLASSERT( start );\r
+        TIXMLASSERT( end );\r
+        Reset();\r
+        _start  = start;\r
+        _end    = end;\r
+        _flags  = flags | NEEDS_FLUSH;\r
+    }\r
+\r
+    const char* GetStr();\r
+\r
+    bool Empty() const {\r
+        return _start == _end;\r
+    }\r
+\r
+    void SetInternedStr( const char* str ) {\r
+        Reset();\r
+        _start = const_cast<char*>(str);\r
+    }\r
+\r
+    void SetStr( const char* str, int flags=0 );\r
+\r
+    char* ParseText( char* in, const char* endTag, int strFlags, int* curLineNumPtr );\r
+    char* ParseName( char* in );\r
+\r
+    void TransferTo( StrPair* other );\r
+       void Reset();\r
+\r
+private:\r
+    void CollapseWhitespace();\r
+\r
+    enum {\r
+        NEEDS_FLUSH = 0x100,\r
+        NEEDS_DELETE = 0x200\r
+    };\r
+\r
+    int     _flags;\r
+    char*   _start;\r
+    char*   _end;\r
+\r
+    StrPair( const StrPair& other );   // not supported\r
+    void operator=( const StrPair& other );    // not supported, use TransferTo()\r
+};\r
+\r
+\r
+/*\r
+       A dynamic array of Plain Old Data. Doesn't support constructors, etc.\r
+       Has a small initial memory pool, so that low or no usage will not\r
+       cause a call to new/delete\r
+*/\r
+template <class T, int INITIAL_SIZE>\r
+class DynArray\r
+{\r
+public:\r
+    DynArray() :\r
+        _mem( _pool ),\r
+        _allocated( INITIAL_SIZE ),\r
+        _size( 0 )\r
+    {\r
+    }\r
+\r
+    ~DynArray() {\r
+        if ( _mem != _pool ) {\r
+            delete [] _mem;\r
+        }\r
+    }\r
+\r
+    void Clear() {\r
+        _size = 0;\r
+    }\r
+\r
+    void Push( T t ) {\r
+        TIXMLASSERT( _size < INT_MAX );\r
+        EnsureCapacity( _size+1 );\r
+        _mem[_size] = t;\r
+        ++_size;\r
+    }\r
+\r
+    T* PushArr( int count ) {\r
+        TIXMLASSERT( count >= 0 );\r
+        TIXMLASSERT( _size <= INT_MAX - count );\r
+        EnsureCapacity( _size+count );\r
+        T* ret = &_mem[_size];\r
+        _size += count;\r
+        return ret;\r
+    }\r
+\r
+    T Pop() {\r
+        TIXMLASSERT( _size > 0 );\r
+        --_size;\r
+        return _mem[_size];\r
+    }\r
+\r
+    void PopArr( int count ) {\r
+        TIXMLASSERT( _size >= count );\r
+        _size -= count;\r
+    }\r
+\r
+    bool Empty() const                                 {\r
+        return _size == 0;\r
+    }\r
+\r
+    T& operator[](int i)                               {\r
+        TIXMLASSERT( i>= 0 && i < _size );\r
+        return _mem[i];\r
+    }\r
+\r
+    const T& operator[](int i) const   {\r
+        TIXMLASSERT( i>= 0 && i < _size );\r
+        return _mem[i];\r
+    }\r
+\r
+    const T& PeekTop() const            {\r
+        TIXMLASSERT( _size > 0 );\r
+        return _mem[ _size - 1];\r
+    }\r
+\r
+    int Size() const                                   {\r
+        TIXMLASSERT( _size >= 0 );\r
+        return _size;\r
+    }\r
+\r
+    int Capacity() const                               {\r
+        TIXMLASSERT( _allocated >= INITIAL_SIZE );\r
+        return _allocated;\r
+    }\r
+\r
+       void SwapRemove(int i) {\r
+               TIXMLASSERT(i >= 0 && i < _size);\r
+               TIXMLASSERT(_size > 0);\r
+               _mem[i] = _mem[_size - 1];\r
+               --_size;\r
+       }\r
+\r
+    const T* Mem() const                               {\r
+        TIXMLASSERT( _mem );\r
+        return _mem;\r
+    }\r
+\r
+    T* Mem() {\r
+        TIXMLASSERT( _mem );\r
+        return _mem;\r
+    }\r
+\r
+private:\r
+    DynArray( const DynArray& ); // not supported\r
+    void operator=( const DynArray& ); // not supported\r
+\r
+    void EnsureCapacity( int cap ) {\r
+        TIXMLASSERT( cap > 0 );\r
+        if ( cap > _allocated ) {\r
+            TIXMLASSERT( cap <= INT_MAX / 2 );\r
+            int newAllocated = cap * 2;\r
+            T* newMem = new T[newAllocated];\r
+            TIXMLASSERT( newAllocated >= _size );\r
+            memcpy( newMem, _mem, sizeof(T)*_size );   // warning: not using constructors, only works for PODs\r
+            if ( _mem != _pool ) {\r
+                delete [] _mem;\r
+            }\r
+            _mem = newMem;\r
+            _allocated = newAllocated;\r
+        }\r
+    }\r
+\r
+    T*  _mem;\r
+    T   _pool[INITIAL_SIZE];\r
+    int _allocated;            // objects allocated\r
+    int _size;                 // number objects in use\r
+};\r
+\r
+\r
+/*\r
+       Parent virtual class of a pool for fast allocation\r
+       and deallocation of objects.\r
+*/\r
+class MemPool\r
+{\r
+public:\r
+    MemPool() {}\r
+    virtual ~MemPool() {}\r
+\r
+    virtual int ItemSize() const = 0;\r
+    virtual void* Alloc() = 0;\r
+    virtual void Free( void* ) = 0;\r
+    virtual void SetTracked() = 0;\r
+};\r
+\r
+\r
+/*\r
+       Template child class to create pools of the correct type.\r
+*/\r
+template< int ITEM_SIZE >\r
+class MemPoolT : public MemPool\r
+{\r
+public:\r
+    MemPoolT() : _blockPtrs(), _root(0), _currentAllocs(0), _nAllocs(0), _maxAllocs(0), _nUntracked(0) {}\r
+    ~MemPoolT() {\r
+        MemPoolT< ITEM_SIZE >::Clear();\r
+    }\r
+\r
+    void Clear() {\r
+        // Delete the blocks.\r
+        while( !_blockPtrs.Empty()) {\r
+            Block* lastBlock = _blockPtrs.Pop();\r
+            delete lastBlock;\r
+        }\r
+        _root = 0;\r
+        _currentAllocs = 0;\r
+        _nAllocs = 0;\r
+        _maxAllocs = 0;\r
+        _nUntracked = 0;\r
+    }\r
+\r
+    virtual int ItemSize() const       {\r
+        return ITEM_SIZE;\r
+    }\r
+    int CurrentAllocs() const          {\r
+        return _currentAllocs;\r
+    }\r
+\r
+    virtual void* Alloc() {\r
+        if ( !_root ) {\r
+            // Need a new block.\r
+            Block* block = new Block();\r
+            _blockPtrs.Push( block );\r
+\r
+            Item* blockItems = block->items;\r
+            for( int i = 0; i < ITEMS_PER_BLOCK - 1; ++i ) {\r
+                blockItems[i].next = &(blockItems[i + 1]);\r
+            }\r
+            blockItems[ITEMS_PER_BLOCK - 1].next = 0;\r
+            _root = blockItems;\r
+        }\r
+        Item* const result = _root;\r
+        TIXMLASSERT( result != 0 );\r
+        _root = _root->next;\r
+\r
+        ++_currentAllocs;\r
+        if ( _currentAllocs > _maxAllocs ) {\r
+            _maxAllocs = _currentAllocs;\r
+        }\r
+        ++_nAllocs;\r
+        ++_nUntracked;\r
+        return result;\r
+    }\r
+\r
+    virtual void Free( void* mem ) {\r
+        if ( !mem ) {\r
+            return;\r
+        }\r
+        --_currentAllocs;\r
+        Item* item = static_cast<Item*>( mem );\r
+#ifdef TINYXML2_DEBUG\r
+        memset( item, 0xfe, sizeof( *item ) );\r
+#endif\r
+        item->next = _root;\r
+        _root = item;\r
+    }\r
+    void Trace( const char* name ) {\r
+        printf( "Mempool %s watermark=%d [%dk] current=%d size=%d nAlloc=%d blocks=%d\n",\r
+                name, _maxAllocs, _maxAllocs * ITEM_SIZE / 1024, _currentAllocs,\r
+                ITEM_SIZE, _nAllocs, _blockPtrs.Size() );\r
+    }\r
+\r
+    void SetTracked() {\r
+        --_nUntracked;\r
+    }\r
+\r
+    int Untracked() const {\r
+        return _nUntracked;\r
+    }\r
+\r
+       // This number is perf sensitive. 4k seems like a good tradeoff on my machine.\r
+       // The test file is large, 170k.\r
+       // Release:             VS2010 gcc(no opt)\r
+       //              1k:             4000\r
+       //              2k:             4000\r
+       //              4k:             3900    21000\r
+       //              16k:    5200\r
+       //              32k:    4300\r
+       //              64k:    4000    21000\r
+    // Declared public because some compilers do not accept to use ITEMS_PER_BLOCK\r
+    // in private part if ITEMS_PER_BLOCK is private\r
+    enum { ITEMS_PER_BLOCK = (4 * 1024) / ITEM_SIZE };\r
+\r
+private:\r
+    MemPoolT( const MemPoolT& ); // not supported\r
+    void operator=( const MemPoolT& ); // not supported\r
+\r
+    union Item {\r
+        Item*   next;\r
+        char    itemData[ITEM_SIZE];\r
+    };\r
+    struct Block {\r
+        Item items[ITEMS_PER_BLOCK];\r
+    };\r
+    DynArray< Block*, 10 > _blockPtrs;\r
+    Item* _root;\r
+\r
+    int _currentAllocs;\r
+    int _nAllocs;\r
+    int _maxAllocs;\r
+    int _nUntracked;\r
+};\r
+\r
+\r
+\r
+/**\r
+       Implements the interface to the "Visitor pattern" (see the Accept() method.)\r
+       If you call the Accept() method, it requires being passed a XMLVisitor\r
+       class to handle callbacks. For nodes that contain other nodes (Document, Element)\r
+       you will get called with a VisitEnter/VisitExit pair. Nodes that are always leafs\r
+       are simply called with Visit().\r
+\r
+       If you return 'true' from a Visit method, recursive parsing will continue. If you return\r
+       false, <b>no children of this node or its siblings</b> will be visited.\r
+\r
+       All flavors of Visit methods have a default implementation that returns 'true' (continue\r
+       visiting). You need to only override methods that are interesting to you.\r
+\r
+       Generally Accept() is called on the XMLDocument, although all nodes support visiting.\r
+\r
+       You should never change the document from a callback.\r
+\r
+       @sa XMLNode::Accept()\r
+*/\r
+class TINYXML2_LIB XMLVisitor\r
+{\r
+public:\r
+    virtual ~XMLVisitor() {}\r
+\r
+    /// Visit a document.\r
+    virtual bool VisitEnter( const XMLDocument& /*doc*/ )                      {\r
+        return true;\r
+    }\r
+    /// Visit a document.\r
+    virtual bool VisitExit( const XMLDocument& /*doc*/ )                       {\r
+        return true;\r
+    }\r
+\r
+    /// Visit an element.\r
+    virtual bool VisitEnter( const XMLElement& /*element*/, const XMLAttribute* /*firstAttribute*/ )   {\r
+        return true;\r
+    }\r
+    /// Visit an element.\r
+    virtual bool VisitExit( const XMLElement& /*element*/ )                    {\r
+        return true;\r
+    }\r
+\r
+    /// Visit a declaration.\r
+    virtual bool Visit( const XMLDeclaration& /*declaration*/ )                {\r
+        return true;\r
+    }\r
+    /// Visit a text node.\r
+    virtual bool Visit( const XMLText& /*text*/ )                                      {\r
+        return true;\r
+    }\r
+    /// Visit a comment node.\r
+    virtual bool Visit( const XMLComment& /*comment*/ )                                {\r
+        return true;\r
+    }\r
+    /// Visit an unknown node.\r
+    virtual bool Visit( const XMLUnknown& /*unknown*/ )                                {\r
+        return true;\r
+    }\r
+};\r
+\r
+// WARNING: must match XMLDocument::_errorNames[]\r
+enum XMLError {\r
+    XML_SUCCESS = 0,\r
+    XML_NO_ATTRIBUTE,\r
+    XML_WRONG_ATTRIBUTE_TYPE,\r
+    XML_ERROR_FILE_NOT_FOUND,\r
+    XML_ERROR_FILE_COULD_NOT_BE_OPENED,\r
+    XML_ERROR_FILE_READ_ERROR,\r
+    XML_ERROR_PARSING_ELEMENT,\r
+    XML_ERROR_PARSING_ATTRIBUTE,\r
+    XML_ERROR_PARSING_TEXT,\r
+    XML_ERROR_PARSING_CDATA,\r
+    XML_ERROR_PARSING_COMMENT,\r
+    XML_ERROR_PARSING_DECLARATION,\r
+    XML_ERROR_PARSING_UNKNOWN,\r
+    XML_ERROR_EMPTY_DOCUMENT,\r
+    XML_ERROR_MISMATCHED_ELEMENT,\r
+    XML_ERROR_PARSING,\r
+    XML_CAN_NOT_CONVERT_TEXT,\r
+    XML_NO_TEXT_NODE,\r
+       XML_ELEMENT_DEPTH_EXCEEDED,\r
+\r
+       XML_ERROR_COUNT\r
+};\r
+\r
+\r
+/*\r
+       Utility functionality.\r
+*/\r
+class TINYXML2_LIB XMLUtil\r
+{\r
+public:\r
+    static const char* SkipWhiteSpace( const char* p, int* curLineNumPtr )     {\r
+        TIXMLASSERT( p );\r
+\r
+        while( IsWhiteSpace(*p) ) {\r
+            if (curLineNumPtr && *p == '\n') {\r
+                ++(*curLineNumPtr);\r
+            }\r
+            ++p;\r
+        }\r
+        TIXMLASSERT( p );\r
+        return p;\r
+    }\r
+    static char* SkipWhiteSpace( char* p, int* curLineNumPtr )                         {\r
+        return const_cast<char*>( SkipWhiteSpace( const_cast<const char*>(p), curLineNumPtr ) );\r
+    }\r
+\r
+    // Anything in the high order range of UTF-8 is assumed to not be whitespace. This isn't\r
+    // correct, but simple, and usually works.\r
+    static bool IsWhiteSpace( char p )                                 {\r
+        return !IsUTF8Continuation(p) && isspace( static_cast<unsigned char>(p) );\r
+    }\r
+\r
+    inline static bool IsNameStartChar( unsigned char ch ) {\r
+        if ( ch >= 128 ) {\r
+            // This is a heuristic guess in attempt to not implement Unicode-aware isalpha()\r
+            return true;\r
+        }\r
+        if ( isalpha( ch ) ) {\r
+            return true;\r
+        }\r
+        return ch == ':' || ch == '_';\r
+    }\r
+\r
+    inline static bool IsNameChar( unsigned char ch ) {\r
+        return IsNameStartChar( ch )\r
+               || isdigit( ch )\r
+               || ch == '.'\r
+               || ch == '-';\r
+    }\r
+\r
+    inline static bool StringEqual( const char* p, const char* q, int nChar=INT_MAX )  {\r
+        if ( p == q ) {\r
+            return true;\r
+        }\r
+        TIXMLASSERT( p );\r
+        TIXMLASSERT( q );\r
+        TIXMLASSERT( nChar >= 0 );\r
+        return strncmp( p, q, nChar ) == 0;\r
+    }\r
+\r
+    inline static bool IsUTF8Continuation( char p ) {\r
+        return ( p & 0x80 ) != 0;\r
+    }\r
+\r
+    static const char* ReadBOM( const char* p, bool* hasBOM );\r
+    // p is the starting location,\r
+    // the UTF-8 value of the entity will be placed in value, and length filled in.\r
+    static const char* GetCharacterRef( const char* p, char* value, int* length );\r
+    static void ConvertUTF32ToUTF8( unsigned long input, char* output, int* length );\r
+\r
+    // converts primitive types to strings\r
+    static void ToStr( int v, char* buffer, int bufferSize );\r
+    static void ToStr( unsigned v, char* buffer, int bufferSize );\r
+    static void ToStr( bool v, char* buffer, int bufferSize );\r
+    static void ToStr( float v, char* buffer, int bufferSize );\r
+    static void ToStr( double v, char* buffer, int bufferSize );\r
+       static void ToStr(int64_t v, char* buffer, int bufferSize);\r
+\r
+    // converts strings to primitive types\r
+    static bool        ToInt( const char* str, int* value );\r
+    static bool ToUnsigned( const char* str, unsigned* value );\r
+    static bool        ToBool( const char* str, bool* value );\r
+    static bool        ToFloat( const char* str, float* value );\r
+    static bool ToDouble( const char* str, double* value );\r
+       static bool ToInt64(const char* str, int64_t* value);\r
+\r
+       // Changes what is serialized for a boolean value.\r
+       // Default to "true" and "false". Shouldn't be changed\r
+       // unless you have a special testing or compatibility need.\r
+       // Be careful: static, global, & not thread safe.\r
+       // Be sure to set static const memory as parameters.\r
+       static void SetBoolSerialization(const char* writeTrue, const char* writeFalse);\r
+\r
+private:\r
+       static const char* writeBoolTrue;\r
+       static const char* writeBoolFalse;\r
+};\r
+\r
+\r
+/** XMLNode is a base class for every object that is in the\r
+       XML Document Object Model (DOM), except XMLAttributes.\r
+       Nodes have siblings, a parent, and children which can\r
+       be navigated. A node is always in a XMLDocument.\r
+       The type of a XMLNode can be queried, and it can\r
+       be cast to its more defined type.\r
+\r
+       A XMLDocument allocates memory for all its Nodes.\r
+       When the XMLDocument gets deleted, all its Nodes\r
+       will also be deleted.\r
+\r
+       @verbatim\r
+       A Document can contain: Element (container or leaf)\r
+                                                       Comment (leaf)\r
+                                                       Unknown (leaf)\r
+                                                       Declaration( leaf )\r
+\r
+       An Element can contain: Element (container or leaf)\r
+                                                       Text    (leaf)\r
+                                                       Attributes (not on tree)\r
+                                                       Comment (leaf)\r
+                                                       Unknown (leaf)\r
+\r
+       @endverbatim\r
+*/\r
+class TINYXML2_LIB XMLNode\r
+{\r
+    friend class XMLDocument;\r
+    friend class XMLElement;\r
+public:\r
+\r
+    /// Get the XMLDocument that owns this XMLNode.\r
+    const XMLDocument* GetDocument() const     {\r
+        TIXMLASSERT( _document );\r
+        return _document;\r
+    }\r
+    /// Get the XMLDocument that owns this XMLNode.\r
+    XMLDocument* GetDocument()                         {\r
+        TIXMLASSERT( _document );\r
+        return _document;\r
+    }\r
+\r
+    /// Safely cast to an Element, or null.\r
+    virtual XMLElement*                ToElement()             {\r
+        return 0;\r
+    }\r
+    /// Safely cast to Text, or null.\r
+    virtual XMLText*           ToText()                {\r
+        return 0;\r
+    }\r
+    /// Safely cast to a Comment, or null.\r
+    virtual XMLComment*                ToComment()             {\r
+        return 0;\r
+    }\r
+    /// Safely cast to a Document, or null.\r
+    virtual XMLDocument*       ToDocument()    {\r
+        return 0;\r
+    }\r
+    /// Safely cast to a Declaration, or null.\r
+    virtual XMLDeclaration*    ToDeclaration() {\r
+        return 0;\r
+    }\r
+    /// Safely cast to an Unknown, or null.\r
+    virtual XMLUnknown*                ToUnknown()             {\r
+        return 0;\r
+    }\r
+\r
+    virtual const XMLElement*          ToElement() const               {\r
+        return 0;\r
+    }\r
+    virtual const XMLText*                     ToText() const                  {\r
+        return 0;\r
+    }\r
+    virtual const XMLComment*          ToComment() const               {\r
+        return 0;\r
+    }\r
+    virtual const XMLDocument*         ToDocument() const              {\r
+        return 0;\r
+    }\r
+    virtual const XMLDeclaration*      ToDeclaration() const   {\r
+        return 0;\r
+    }\r
+    virtual const XMLUnknown*          ToUnknown() const               {\r
+        return 0;\r
+    }\r
+\r
+    /** The meaning of 'value' changes for the specific type.\r
+       @verbatim\r
+       Document:       empty (NULL is returned, not an empty string)\r
+       Element:        name of the element\r
+       Comment:        the comment text\r
+       Unknown:        the tag contents\r
+       Text:           the text string\r
+       @endverbatim\r
+    */\r
+    const char* Value() const;\r
+\r
+    /** Set the Value of an XML node.\r
+       @sa Value()\r
+    */\r
+    void SetValue( const char* val, bool staticMem=false );\r
+\r
+    /// Gets the line number the node is in, if the document was parsed from a file.\r
+    int GetLineNum() const { return _parseLineNum; }\r
+\r
+    /// Get the parent of this node on the DOM.\r
+    const XMLNode*     Parent() const                  {\r
+        return _parent;\r
+    }\r
+\r
+    XMLNode* Parent()                                          {\r
+        return _parent;\r
+    }\r
+\r
+    /// Returns true if this node has no children.\r
+    bool NoChildren() const                                    {\r
+        return !_firstChild;\r
+    }\r
+\r
+    /// Get the first child node, or null if none exists.\r
+    const XMLNode*  FirstChild() const         {\r
+        return _firstChild;\r
+    }\r
+\r
+    XMLNode*           FirstChild()                    {\r
+        return _firstChild;\r
+    }\r
+\r
+    /** Get the first child element, or optionally the first child\r
+        element with the specified name.\r
+    */\r
+    const XMLElement* FirstChildElement( const char* name = 0 ) const;\r
+\r
+    XMLElement* FirstChildElement( const char* name = 0 )      {\r
+        return const_cast<XMLElement*>(const_cast<const XMLNode*>(this)->FirstChildElement( name ));\r
+    }\r
+\r
+    /// Get the last child node, or null if none exists.\r
+    const XMLNode*     LastChild() const                                               {\r
+        return _lastChild;\r
+    }\r
+\r
+    XMLNode*           LastChild()                                                             {\r
+        return _lastChild;\r
+    }\r
+\r
+    /** Get the last child element or optionally the last child\r
+        element with the specified name.\r
+    */\r
+    const XMLElement* LastChildElement( const char* name = 0 ) const;\r
+\r
+    XMLElement* LastChildElement( const char* name = 0 )       {\r
+        return const_cast<XMLElement*>(const_cast<const XMLNode*>(this)->LastChildElement(name) );\r
+    }\r
+\r
+    /// Get the previous (left) sibling node of this node.\r
+    const XMLNode*     PreviousSibling() const                                 {\r
+        return _prev;\r
+    }\r
+\r
+    XMLNode*   PreviousSibling()                                                       {\r
+        return _prev;\r
+    }\r
+\r
+    /// Get the previous (left) sibling element of this node, with an optionally supplied name.\r
+    const XMLElement*  PreviousSiblingElement( const char* name = 0 ) const ;\r
+\r
+    XMLElement*        PreviousSiblingElement( const char* name = 0 ) {\r
+        return const_cast<XMLElement*>(const_cast<const XMLNode*>(this)->PreviousSiblingElement( name ) );\r
+    }\r
+\r
+    /// Get the next (right) sibling node of this node.\r
+    const XMLNode*     NextSibling() const                                             {\r
+        return _next;\r
+    }\r
+\r
+    XMLNode*   NextSibling()                                                           {\r
+        return _next;\r
+    }\r
+\r
+    /// Get the next (right) sibling element of this node, with an optionally supplied name.\r
+    const XMLElement*  NextSiblingElement( const char* name = 0 ) const;\r
+\r
+    XMLElement*        NextSiblingElement( const char* name = 0 )      {\r
+        return const_cast<XMLElement*>(const_cast<const XMLNode*>(this)->NextSiblingElement( name ) );\r
+    }\r
+\r
+    /**\r
+       Add a child node as the last (right) child.\r
+               If the child node is already part of the document,\r
+               it is moved from its old location to the new location.\r
+               Returns the addThis argument or 0 if the node does not\r
+               belong to the same document.\r
+    */\r
+    XMLNode* InsertEndChild( XMLNode* addThis );\r
+\r
+    XMLNode* LinkEndChild( XMLNode* addThis )  {\r
+        return InsertEndChild( addThis );\r
+    }\r
+    /**\r
+       Add a child node as the first (left) child.\r
+               If the child node is already part of the document,\r
+               it is moved from its old location to the new location.\r
+               Returns the addThis argument or 0 if the node does not\r
+               belong to the same document.\r
+    */\r
+    XMLNode* InsertFirstChild( XMLNode* addThis );\r
+    /**\r
+       Add a node after the specified child node.\r
+               If the child node is already part of the document,\r
+               it is moved from its old location to the new location.\r
+               Returns the addThis argument or 0 if the afterThis node\r
+               is not a child of this node, or if the node does not\r
+               belong to the same document.\r
+    */\r
+    XMLNode* InsertAfterChild( XMLNode* afterThis, XMLNode* addThis );\r
+\r
+    /**\r
+       Delete all the children of this node.\r
+    */\r
+    void DeleteChildren();\r
+\r
+    /**\r
+       Delete a child of this node.\r
+    */\r
+    void DeleteChild( XMLNode* node );\r
+\r
+    /**\r
+       Make a copy of this node, but not its children.\r
+       You may pass in a Document pointer that will be\r
+       the owner of the new Node. If the 'document' is\r
+       null, then the node returned will be allocated\r
+       from the current Document. (this->GetDocument())\r
+\r
+       Note: if called on a XMLDocument, this will return null.\r
+    */\r
+    virtual XMLNode* ShallowClone( XMLDocument* document ) const = 0;\r
+\r
+       /**\r
+               Make a copy of this node and all its children.\r
+\r
+               If the 'target' is null, then the nodes will\r
+               be allocated in the current document. If 'target'\r
+        is specified, the memory will be allocated is the\r
+        specified XMLDocument.\r
+\r
+               NOTE: This is probably not the correct tool to\r
+               copy a document, since XMLDocuments can have multiple\r
+               top level XMLNodes. You probably want to use\r
+        XMLDocument::DeepCopy()\r
+       */\r
+       XMLNode* DeepClone( XMLDocument* target ) const;\r
+\r
+    /**\r
+       Test if 2 nodes are the same, but don't test children.\r
+       The 2 nodes do not need to be in the same Document.\r
+\r
+       Note: if called on a XMLDocument, this will return false.\r
+    */\r
+    virtual bool ShallowEqual( const XMLNode* compare ) const = 0;\r
+\r
+    /** Accept a hierarchical visit of the nodes in the TinyXML-2 DOM. Every node in the\r
+       XML tree will be conditionally visited and the host will be called back\r
+       via the XMLVisitor interface.\r
+\r
+       This is essentially a SAX interface for TinyXML-2. (Note however it doesn't re-parse\r
+       the XML for the callbacks, so the performance of TinyXML-2 is unchanged by using this\r
+       interface versus any other.)\r
+\r
+       The interface has been based on ideas from:\r
+\r
+       - http://www.saxproject.org/\r
+       - http://c2.com/cgi/wiki?HierarchicalVisitorPattern\r
+\r
+       Which are both good references for "visiting".\r
+\r
+       An example of using Accept():\r
+       @verbatim\r
+       XMLPrinter printer;\r
+       tinyxmlDoc.Accept( &printer );\r
+       const char* xmlcstr = printer.CStr();\r
+       @endverbatim\r
+    */\r
+    virtual bool Accept( XMLVisitor* visitor ) const = 0;\r
+\r
+       /**\r
+               Set user data into the XMLNode. TinyXML-2 in\r
+               no way processes or interprets user data.\r
+               It is initially 0.\r
+       */\r
+       void SetUserData(void* userData)        { _userData = userData; }\r
+\r
+       /**\r
+               Get user data set into the XMLNode. TinyXML-2 in\r
+               no way processes or interprets user data.\r
+               It is initially 0.\r
+       */\r
+       void* GetUserData() const                       { return _userData; }\r
+\r
+protected:\r
+    explicit XMLNode( XMLDocument* );\r
+    virtual ~XMLNode();\r
+\r
+    virtual char* ParseDeep( char* p, StrPair* parentEndTag, int* curLineNumPtr);\r
+\r
+    XMLDocument*       _document;\r
+    XMLNode*           _parent;\r
+    mutable StrPair    _value;\r
+    int             _parseLineNum;\r
+\r
+    XMLNode*           _firstChild;\r
+    XMLNode*           _lastChild;\r
+\r
+    XMLNode*           _prev;\r
+    XMLNode*           _next;\r
+\r
+       void*                   _userData;\r
+\r
+private:\r
+    MemPool*           _memPool;\r
+    void Unlink( XMLNode* child );\r
+    static void DeleteNode( XMLNode* node );\r
+    void InsertChildPreamble( XMLNode* insertThis ) const;\r
+    const XMLElement* ToElementWithName( const char* name ) const;\r
+\r
+    XMLNode( const XMLNode& ); // not supported\r
+    XMLNode& operator=( const XMLNode& );      // not supported\r
+};\r
+\r
+\r
+/** XML text.\r
+\r
+       Note that a text node can have child element nodes, for example:\r
+       @verbatim\r
+       <root>This is <b>bold</b></root>\r
+       @endverbatim\r
+\r
+       A text node can have 2 ways to output the next. "normal" output\r
+       and CDATA. It will default to the mode it was parsed from the XML file and\r
+       you generally want to leave it alone, but you can change the output mode with\r
+       SetCData() and query it with CData().\r
+*/\r
+class TINYXML2_LIB XMLText : public XMLNode\r
+{\r
+    friend class XMLDocument;\r
+public:\r
+    virtual bool Accept( XMLVisitor* visitor ) const;\r
+\r
+    virtual XMLText* ToText()                  {\r
+        return this;\r
+    }\r
+    virtual const XMLText* ToText() const      {\r
+        return this;\r
+    }\r
+\r
+    /// Declare whether this should be CDATA or standard text.\r
+    void SetCData( bool isCData )                      {\r
+        _isCData = isCData;\r
+    }\r
+    /// Returns true if this is a CDATA text element.\r
+    bool CData() const                                         {\r
+        return _isCData;\r
+    }\r
+\r
+    virtual XMLNode* ShallowClone( XMLDocument* document ) const;\r
+    virtual bool ShallowEqual( const XMLNode* compare ) const;\r
+\r
+protected:\r
+    explicit XMLText( XMLDocument* doc )       : XMLNode( doc ), _isCData( false )     {}\r
+    virtual ~XMLText()                                                                                         {}\r
+\r
+    char* ParseDeep( char* p, StrPair* parentEndTag, int* curLineNumPtr );\r
+\r
+private:\r
+    bool _isCData;\r
+\r
+    XMLText( const XMLText& ); // not supported\r
+    XMLText& operator=( const XMLText& );      // not supported\r
+};\r
+\r
+\r
+/** An XML Comment. */\r
+class TINYXML2_LIB XMLComment : public XMLNode\r
+{\r
+    friend class XMLDocument;\r
+public:\r
+    virtual XMLComment*        ToComment()                                     {\r
+        return this;\r
+    }\r
+    virtual const XMLComment* ToComment() const                {\r
+        return this;\r
+    }\r
+\r
+    virtual bool Accept( XMLVisitor* visitor ) const;\r
+\r
+    virtual XMLNode* ShallowClone( XMLDocument* document ) const;\r
+    virtual bool ShallowEqual( const XMLNode* compare ) const;\r
+\r
+protected:\r
+    explicit XMLComment( XMLDocument* doc );\r
+    virtual ~XMLComment();\r
+\r
+    char* ParseDeep( char* p, StrPair* parentEndTag, int* curLineNumPtr);\r
+\r
+private:\r
+    XMLComment( const XMLComment& );   // not supported\r
+    XMLComment& operator=( const XMLComment& );        // not supported\r
+};\r
+\r
+\r
+/** In correct XML the declaration is the first entry in the file.\r
+       @verbatim\r
+               <?xml version="1.0" standalone="yes"?>\r
+       @endverbatim\r
+\r
+       TinyXML-2 will happily read or write files without a declaration,\r
+       however.\r
+\r
+       The text of the declaration isn't interpreted. It is parsed\r
+       and written as a string.\r
+*/\r
+class TINYXML2_LIB XMLDeclaration : public XMLNode\r
+{\r
+    friend class XMLDocument;\r
+public:\r
+    virtual XMLDeclaration*    ToDeclaration()                                 {\r
+        return this;\r
+    }\r
+    virtual const XMLDeclaration* ToDeclaration() const                {\r
+        return this;\r
+    }\r
+\r
+    virtual bool Accept( XMLVisitor* visitor ) const;\r
+\r
+    virtual XMLNode* ShallowClone( XMLDocument* document ) const;\r
+    virtual bool ShallowEqual( const XMLNode* compare ) const;\r
+\r
+protected:\r
+    explicit XMLDeclaration( XMLDocument* doc );\r
+    virtual ~XMLDeclaration();\r
+\r
+    char* ParseDeep( char* p, StrPair* parentEndTag, int* curLineNumPtr );\r
+\r
+private:\r
+    XMLDeclaration( const XMLDeclaration& );   // not supported\r
+    XMLDeclaration& operator=( const XMLDeclaration& );        // not supported\r
+};\r
+\r
+\r
+/** Any tag that TinyXML-2 doesn't recognize is saved as an\r
+       unknown. It is a tag of text, but should not be modified.\r
+       It will be written back to the XML, unchanged, when the file\r
+       is saved.\r
+\r
+       DTD tags get thrown into XMLUnknowns.\r
+*/\r
+class TINYXML2_LIB XMLUnknown : public XMLNode\r
+{\r
+    friend class XMLDocument;\r
+public:\r
+    virtual XMLUnknown*        ToUnknown()                                     {\r
+        return this;\r
+    }\r
+    virtual const XMLUnknown* ToUnknown() const                {\r
+        return this;\r
+    }\r
+\r
+    virtual bool Accept( XMLVisitor* visitor ) const;\r
+\r
+    virtual XMLNode* ShallowClone( XMLDocument* document ) const;\r
+    virtual bool ShallowEqual( const XMLNode* compare ) const;\r
+\r
+protected:\r
+    explicit XMLUnknown( XMLDocument* doc );\r
+    virtual ~XMLUnknown();\r
+\r
+    char* ParseDeep( char* p, StrPair* parentEndTag, int* curLineNumPtr );\r
+\r
+private:\r
+    XMLUnknown( const XMLUnknown& );   // not supported\r
+    XMLUnknown& operator=( const XMLUnknown& );        // not supported\r
+};\r
+\r
+\r
+\r
+/** An attribute is a name-value pair. Elements have an arbitrary\r
+       number of attributes, each with a unique name.\r
+\r
+       @note The attributes are not XMLNodes. You may only query the\r
+       Next() attribute in a list.\r
+*/\r
+class TINYXML2_LIB XMLAttribute\r
+{\r
+    friend class XMLElement;\r
+public:\r
+    /// The name of the attribute.\r
+    const char* Name() const;\r
+\r
+    /// The value of the attribute.\r
+    const char* Value() const;\r
+\r
+    /// Gets the line number the attribute is in, if the document was parsed from a file.\r
+    int GetLineNum() const { return _parseLineNum; }\r
+\r
+    /// The next attribute in the list.\r
+    const XMLAttribute* Next() const {\r
+        return _next;\r
+    }\r
+\r
+    /** IntValue interprets the attribute as an integer, and returns the value.\r
+        If the value isn't an integer, 0 will be returned. There is no error checking;\r
+       use QueryIntValue() if you need error checking.\r
+    */\r
+       int     IntValue() const {\r
+               int i = 0;\r
+               QueryIntValue(&i);\r
+               return i;\r
+       }\r
+\r
+       int64_t Int64Value() const {\r
+               int64_t i = 0;\r
+               QueryInt64Value(&i);\r
+               return i;\r
+       }\r
+\r
+    /// Query as an unsigned integer. See IntValue()\r
+    unsigned UnsignedValue() const                     {\r
+        unsigned i=0;\r
+        QueryUnsignedValue( &i );\r
+        return i;\r
+    }\r
+    /// Query as a boolean. See IntValue()\r
+    bool        BoolValue() const                              {\r
+        bool b=false;\r
+        QueryBoolValue( &b );\r
+        return b;\r
+    }\r
+    /// Query as a double. See IntValue()\r
+    double      DoubleValue() const                    {\r
+        double d=0;\r
+        QueryDoubleValue( &d );\r
+        return d;\r
+    }\r
+    /// Query as a float. See IntValue()\r
+    float       FloatValue() const                             {\r
+        float f=0;\r
+        QueryFloatValue( &f );\r
+        return f;\r
+    }\r
+\r
+    /** QueryIntValue interprets the attribute as an integer, and returns the value\r
+       in the provided parameter. The function will return XML_SUCCESS on success,\r
+       and XML_WRONG_ATTRIBUTE_TYPE if the conversion is not successful.\r
+    */\r
+    XMLError QueryIntValue( int* value ) const;\r
+    /// See QueryIntValue\r
+    XMLError QueryUnsignedValue( unsigned int* value ) const;\r
+       /// See QueryIntValue\r
+       XMLError QueryInt64Value(int64_t* value) const;\r
+       /// See QueryIntValue\r
+    XMLError QueryBoolValue( bool* value ) const;\r
+    /// See QueryIntValue\r
+    XMLError QueryDoubleValue( double* value ) const;\r
+    /// See QueryIntValue\r
+    XMLError QueryFloatValue( float* value ) const;\r
+\r
+    /// Set the attribute to a string value.\r
+    void SetAttribute( const char* value );\r
+    /// Set the attribute to value.\r
+    void SetAttribute( int value );\r
+    /// Set the attribute to value.\r
+    void SetAttribute( unsigned value );\r
+       /// Set the attribute to value.\r
+       void SetAttribute(int64_t value);\r
+       /// Set the attribute to value.\r
+    void SetAttribute( bool value );\r
+    /// Set the attribute to value.\r
+    void SetAttribute( double value );\r
+    /// Set the attribute to value.\r
+    void SetAttribute( float value );\r
+\r
+private:\r
+    enum { BUF_SIZE = 200 };\r
+\r
+    XMLAttribute() : _name(), _value(),_parseLineNum( 0 ), _next( 0 ), _memPool( 0 ) {}\r
+    virtual ~XMLAttribute()    {}\r
+\r
+    XMLAttribute( const XMLAttribute& );       // not supported\r
+    void operator=( const XMLAttribute& );     // not supported\r
+    void SetName( const char* name );\r
+\r
+    char* ParseDeep( char* p, bool processEntities, int* curLineNumPtr );\r
+\r
+    mutable StrPair _name;\r
+    mutable StrPair _value;\r
+    int             _parseLineNum;\r
+    XMLAttribute*   _next;\r
+    MemPool*        _memPool;\r
+};\r
+\r
+\r
+/** The element is a container class. It has a value, the element name,\r
+       and can contain other elements, text, comments, and unknowns.\r
+       Elements also contain an arbitrary number of attributes.\r
+*/\r
+class TINYXML2_LIB XMLElement : public XMLNode\r
+{\r
+    friend class XMLDocument;\r
+public:\r
+    /// Get the name of an element (which is the Value() of the node.)\r
+    const char* Name() const           {\r
+        return Value();\r
+    }\r
+    /// Set the name of the element.\r
+    void SetName( const char* str, bool staticMem=false )      {\r
+        SetValue( str, staticMem );\r
+    }\r
+\r
+    virtual XMLElement* ToElement()                            {\r
+        return this;\r
+    }\r
+    virtual const XMLElement* ToElement() const {\r
+        return this;\r
+    }\r
+    virtual bool Accept( XMLVisitor* visitor ) const;\r
+\r
+    /** Given an attribute name, Attribute() returns the value\r
+       for the attribute of that name, or null if none\r
+       exists. For example:\r
+\r
+       @verbatim\r
+       const char* value = ele->Attribute( "foo" );\r
+       @endverbatim\r
+\r
+       The 'value' parameter is normally null. However, if specified,\r
+       the attribute will only be returned if the 'name' and 'value'\r
+       match. This allow you to write code:\r
+\r
+       @verbatim\r
+       if ( ele->Attribute( "foo", "bar" ) ) callFooIsBar();\r
+       @endverbatim\r
+\r
+       rather than:\r
+       @verbatim\r
+       if ( ele->Attribute( "foo" ) ) {\r
+               if ( strcmp( ele->Attribute( "foo" ), "bar" ) == 0 ) callFooIsBar();\r
+       }\r
+       @endverbatim\r
+    */\r
+    const char* Attribute( const char* name, const char* value=0 ) const;\r
+\r
+    /** Given an attribute name, IntAttribute() returns the value\r
+       of the attribute interpreted as an integer. The default\r
+        value will be returned if the attribute isn't present,\r
+        or if there is an error. (For a method with error\r
+       checking, see QueryIntAttribute()).\r
+    */\r
+       int IntAttribute(const char* name, int defaultValue = 0) const;\r
+    /// See IntAttribute()\r
+       unsigned UnsignedAttribute(const char* name, unsigned defaultValue = 0) const;\r
+       /// See IntAttribute()\r
+       int64_t Int64Attribute(const char* name, int64_t defaultValue = 0) const;\r
+       /// See IntAttribute()\r
+       bool BoolAttribute(const char* name, bool defaultValue = false) const;\r
+    /// See IntAttribute()\r
+       double DoubleAttribute(const char* name, double defaultValue = 0) const;\r
+    /// See IntAttribute()\r
+       float FloatAttribute(const char* name, float defaultValue = 0) const;\r
+\r
+    /** Given an attribute name, QueryIntAttribute() returns\r
+       XML_SUCCESS, XML_WRONG_ATTRIBUTE_TYPE if the conversion\r
+       can't be performed, or XML_NO_ATTRIBUTE if the attribute\r
+       doesn't exist. If successful, the result of the conversion\r
+       will be written to 'value'. If not successful, nothing will\r
+       be written to 'value'. This allows you to provide default\r
+       value:\r
+\r
+       @verbatim\r
+       int value = 10;\r
+       QueryIntAttribute( "foo", &value );             // if "foo" isn't found, value will still be 10\r
+       @endverbatim\r
+    */\r
+    XMLError QueryIntAttribute( const char* name, int* value ) const                           {\r
+        const XMLAttribute* a = FindAttribute( name );\r
+        if ( !a ) {\r
+            return XML_NO_ATTRIBUTE;\r
+        }\r
+        return a->QueryIntValue( value );\r
+    }\r
+\r
+       /// See QueryIntAttribute()\r
+    XMLError QueryUnsignedAttribute( const char* name, unsigned int* value ) const     {\r
+        const XMLAttribute* a = FindAttribute( name );\r
+        if ( !a ) {\r
+            return XML_NO_ATTRIBUTE;\r
+        }\r
+        return a->QueryUnsignedValue( value );\r
+    }\r
+\r
+       /// See QueryIntAttribute()\r
+       XMLError QueryInt64Attribute(const char* name, int64_t* value) const {\r
+               const XMLAttribute* a = FindAttribute(name);\r
+               if (!a) {\r
+                       return XML_NO_ATTRIBUTE;\r
+               }\r
+               return a->QueryInt64Value(value);\r
+       }\r
+\r
+       /// See QueryIntAttribute()\r
+    XMLError QueryBoolAttribute( const char* name, bool* value ) const                         {\r
+        const XMLAttribute* a = FindAttribute( name );\r
+        if ( !a ) {\r
+            return XML_NO_ATTRIBUTE;\r
+        }\r
+        return a->QueryBoolValue( value );\r
+    }\r
+    /// See QueryIntAttribute()\r
+    XMLError QueryDoubleAttribute( const char* name, double* value ) const                     {\r
+        const XMLAttribute* a = FindAttribute( name );\r
+        if ( !a ) {\r
+            return XML_NO_ATTRIBUTE;\r
+        }\r
+        return a->QueryDoubleValue( value );\r
+    }\r
+    /// See QueryIntAttribute()\r
+    XMLError QueryFloatAttribute( const char* name, float* value ) const                       {\r
+        const XMLAttribute* a = FindAttribute( name );\r
+        if ( !a ) {\r
+            return XML_NO_ATTRIBUTE;\r
+        }\r
+        return a->QueryFloatValue( value );\r
+    }\r
+\r
+       /// See QueryIntAttribute()\r
+       XMLError QueryStringAttribute(const char* name, const char** value) const {\r
+               const XMLAttribute* a = FindAttribute(name);\r
+               if (!a) {\r
+                       return XML_NO_ATTRIBUTE;\r
+               }\r
+               *value = a->Value();\r
+               return XML_SUCCESS;\r
+       }\r
+\r
+\r
+\r
+    /** Given an attribute name, QueryAttribute() returns\r
+       XML_SUCCESS, XML_WRONG_ATTRIBUTE_TYPE if the conversion\r
+       can't be performed, or XML_NO_ATTRIBUTE if the attribute\r
+       doesn't exist. It is overloaded for the primitive types,\r
+               and is a generally more convenient replacement of\r
+               QueryIntAttribute() and related functions.\r
+\r
+               If successful, the result of the conversion\r
+       will be written to 'value'. If not successful, nothing will\r
+       be written to 'value'. This allows you to provide default\r
+       value:\r
+\r
+       @verbatim\r
+       int value = 10;\r
+       QueryAttribute( "foo", &value );                // if "foo" isn't found, value will still be 10\r
+       @endverbatim\r
+    */\r
+       XMLError QueryAttribute( const char* name, int* value ) const {\r
+               return QueryIntAttribute( name, value );\r
+       }\r
+\r
+       XMLError QueryAttribute( const char* name, unsigned int* value ) const {\r
+               return QueryUnsignedAttribute( name, value );\r
+       }\r
+\r
+       XMLError QueryAttribute(const char* name, int64_t* value) const {\r
+               return QueryInt64Attribute(name, value);\r
+       }\r
+\r
+       XMLError QueryAttribute( const char* name, bool* value ) const {\r
+               return QueryBoolAttribute( name, value );\r
+       }\r
+\r
+       XMLError QueryAttribute( const char* name, double* value ) const {\r
+               return QueryDoubleAttribute( name, value );\r
+       }\r
+\r
+       XMLError QueryAttribute( const char* name, float* value ) const {\r
+               return QueryFloatAttribute( name, value );\r
+       }\r
+\r
+       /// Sets the named attribute to value.\r
+    void SetAttribute( const char* name, const char* value )   {\r
+        XMLAttribute* a = FindOrCreateAttribute( name );\r
+        a->SetAttribute( value );\r
+    }\r
+    /// Sets the named attribute to value.\r
+    void SetAttribute( const char* name, int value )                   {\r
+        XMLAttribute* a = FindOrCreateAttribute( name );\r
+        a->SetAttribute( value );\r
+    }\r
+    /// Sets the named attribute to value.\r
+    void SetAttribute( const char* name, unsigned value )              {\r
+        XMLAttribute* a = FindOrCreateAttribute( name );\r
+        a->SetAttribute( value );\r
+    }\r
+\r
+       /// Sets the named attribute to value.\r
+       void SetAttribute(const char* name, int64_t value) {\r
+               XMLAttribute* a = FindOrCreateAttribute(name);\r
+               a->SetAttribute(value);\r
+       }\r
+\r
+       /// Sets the named attribute to value.\r
+    void SetAttribute( const char* name, bool value )                  {\r
+        XMLAttribute* a = FindOrCreateAttribute( name );\r
+        a->SetAttribute( value );\r
+    }\r
+    /// Sets the named attribute to value.\r
+    void SetAttribute( const char* name, double value )                {\r
+        XMLAttribute* a = FindOrCreateAttribute( name );\r
+        a->SetAttribute( value );\r
+    }\r
+    /// Sets the named attribute to value.\r
+    void SetAttribute( const char* name, float value )         {\r
+        XMLAttribute* a = FindOrCreateAttribute( name );\r
+        a->SetAttribute( value );\r
+    }\r
+\r
+    /**\r
+       Delete an attribute.\r
+    */\r
+    void DeleteAttribute( const char* name );\r
+\r
+    /// Return the first attribute in the list.\r
+    const XMLAttribute* FirstAttribute() const {\r
+        return _rootAttribute;\r
+    }\r
+    /// Query a specific attribute in the list.\r
+    const XMLAttribute* FindAttribute( const char* name ) const;\r
+\r
+    /** Convenience function for easy access to the text inside an element. Although easy\r
+       and concise, GetText() is limited compared to getting the XMLText child\r
+       and accessing it directly.\r
+\r
+       If the first child of 'this' is a XMLText, the GetText()\r
+       returns the character string of the Text node, else null is returned.\r
+\r
+       This is a convenient method for getting the text of simple contained text:\r
+       @verbatim\r
+       <foo>This is text</foo>\r
+               const char* str = fooElement->GetText();\r
+       @endverbatim\r
+\r
+       'str' will be a pointer to "This is text".\r
+\r
+       Note that this function can be misleading. If the element foo was created from\r
+       this XML:\r
+       @verbatim\r
+               <foo><b>This is text</b></foo>\r
+       @endverbatim\r
+\r
+       then the value of str would be null. The first child node isn't a text node, it is\r
+       another element. From this XML:\r
+       @verbatim\r
+               <foo>This is <b>text</b></foo>\r
+       @endverbatim\r
+       GetText() will return "This is ".\r
+    */\r
+    const char* GetText() const;\r
+\r
+    /** Convenience function for easy access to the text inside an element. Although easy\r
+       and concise, SetText() is limited compared to creating an XMLText child\r
+       and mutating it directly.\r
+\r
+       If the first child of 'this' is a XMLText, SetText() sets its value to\r
+               the given string, otherwise it will create a first child that is an XMLText.\r
+\r
+       This is a convenient method for setting the text of simple contained text:\r
+       @verbatim\r
+       <foo>This is text</foo>\r
+               fooElement->SetText( "Hullaballoo!" );\r
+       <foo>Hullaballoo!</foo>\r
+               @endverbatim\r
+\r
+       Note that this function can be misleading. If the element foo was created from\r
+       this XML:\r
+       @verbatim\r
+               <foo><b>This is text</b></foo>\r
+       @endverbatim\r
+\r
+       then it will not change "This is text", but rather prefix it with a text element:\r
+       @verbatim\r
+               <foo>Hullaballoo!<b>This is text</b></foo>\r
+       @endverbatim\r
+\r
+               For this XML:\r
+       @verbatim\r
+               <foo />\r
+       @endverbatim\r
+       SetText() will generate\r
+       @verbatim\r
+               <foo>Hullaballoo!</foo>\r
+       @endverbatim\r
+    */\r
+       void SetText( const char* inText );\r
+    /// Convenience method for setting text inside an element. See SetText() for important limitations.\r
+    void SetText( int value );\r
+    /// Convenience method for setting text inside an element. See SetText() for important limitations.\r
+    void SetText( unsigned value );\r
+       /// Convenience method for setting text inside an element. See SetText() for important limitations.\r
+       void SetText(int64_t value);\r
+       /// Convenience method for setting text inside an element. See SetText() for important limitations.\r
+    void SetText( bool value );\r
+    /// Convenience method for setting text inside an element. See SetText() for important limitations.\r
+    void SetText( double value );\r
+    /// Convenience method for setting text inside an element. See SetText() for important limitations.\r
+    void SetText( float value );\r
+\r
+    /**\r
+       Convenience method to query the value of a child text node. This is probably best\r
+       shown by example. Given you have a document is this form:\r
+       @verbatim\r
+               <point>\r
+                       <x>1</x>\r
+                       <y>1.4</y>\r
+               </point>\r
+       @endverbatim\r
+\r
+       The QueryIntText() and similar functions provide a safe and easier way to get to the\r
+       "value" of x and y.\r
+\r
+       @verbatim\r
+               int x = 0;\r
+               float y = 0;    // types of x and y are contrived for example\r
+               const XMLElement* xElement = pointElement->FirstChildElement( "x" );\r
+               const XMLElement* yElement = pointElement->FirstChildElement( "y" );\r
+               xElement->QueryIntText( &x );\r
+               yElement->QueryFloatText( &y );\r
+       @endverbatim\r
+\r
+       @returns XML_SUCCESS (0) on success, XML_CAN_NOT_CONVERT_TEXT if the text cannot be converted\r
+                        to the requested type, and XML_NO_TEXT_NODE if there is no child text to query.\r
+\r
+    */\r
+    XMLError QueryIntText( int* ival ) const;\r
+    /// See QueryIntText()\r
+    XMLError QueryUnsignedText( unsigned* uval ) const;\r
+       /// See QueryIntText()\r
+       XMLError QueryInt64Text(int64_t* uval) const;\r
+       /// See QueryIntText()\r
+    XMLError QueryBoolText( bool* bval ) const;\r
+    /// See QueryIntText()\r
+    XMLError QueryDoubleText( double* dval ) const;\r
+    /// See QueryIntText()\r
+    XMLError QueryFloatText( float* fval ) const;\r
+\r
+       int IntText(int defaultValue = 0) const;\r
+\r
+       /// See QueryIntText()\r
+       unsigned UnsignedText(unsigned defaultValue = 0) const;\r
+       /// See QueryIntText()\r
+       int64_t Int64Text(int64_t defaultValue = 0) const;\r
+       /// See QueryIntText()\r
+       bool BoolText(bool defaultValue = false) const;\r
+       /// See QueryIntText()\r
+       double DoubleText(double defaultValue = 0) const;\r
+       /// See QueryIntText()\r
+       float FloatText(float defaultValue = 0) const;\r
+\r
+    // internal:\r
+    enum ElementClosingType {\r
+        OPEN,          // <foo>\r
+        CLOSED,                // <foo/>\r
+        CLOSING                // </foo>\r
+    };\r
+    ElementClosingType ClosingType() const {\r
+        return _closingType;\r
+    }\r
+    virtual XMLNode* ShallowClone( XMLDocument* document ) const;\r
+    virtual bool ShallowEqual( const XMLNode* compare ) const;\r
+\r
+protected:\r
+    char* ParseDeep( char* p, StrPair* parentEndTag, int* curLineNumPtr );\r
+\r
+private:\r
+    XMLElement( XMLDocument* doc );\r
+    virtual ~XMLElement();\r
+    XMLElement( const XMLElement& );   // not supported\r
+    void operator=( const XMLElement& );       // not supported\r
+\r
+    XMLAttribute* FindOrCreateAttribute( const char* name );\r
+    char* ParseAttributes( char* p, int* curLineNumPtr );\r
+    static void DeleteAttribute( XMLAttribute* attribute );\r
+    XMLAttribute* CreateAttribute();\r
+\r
+    enum { BUF_SIZE = 200 };\r
+    ElementClosingType _closingType;\r
+    // The attribute list is ordered; there is no 'lastAttribute'\r
+    // because the list needs to be scanned for dupes before adding\r
+    // a new attribute.\r
+    XMLAttribute* _rootAttribute;\r
+};\r
+\r
+\r
+enum Whitespace {\r
+    PRESERVE_WHITESPACE,\r
+    COLLAPSE_WHITESPACE\r
+};\r
+\r
+\r
+/** A Document binds together all the functionality.\r
+       It can be saved, loaded, and printed to the screen.\r
+       All Nodes are connected and allocated to a Document.\r
+       If the Document is deleted, all its Nodes are also deleted.\r
+*/\r
+class TINYXML2_LIB XMLDocument : public XMLNode\r
+{\r
+    friend class XMLElement;\r
+    // Gives access to SetError and Push/PopDepth, but over-access for everything else.\r
+    // Wishing C++ had "internal" scope.\r
+    friend class XMLNode;\r
+    friend class XMLText;\r
+    friend class XMLComment;\r
+    friend class XMLDeclaration;\r
+    friend class XMLUnknown;\r
+public:\r
+    /// constructor\r
+    XMLDocument( bool processEntities = true, Whitespace whitespaceMode = PRESERVE_WHITESPACE );\r
+    ~XMLDocument();\r
+\r
+    virtual XMLDocument* ToDocument()                          {\r
+        TIXMLASSERT( this == _document );\r
+        return this;\r
+    }\r
+    virtual const XMLDocument* ToDocument() const      {\r
+        TIXMLASSERT( this == _document );\r
+        return this;\r
+    }\r
+\r
+    /**\r
+       Parse an XML file from a character string.\r
+       Returns XML_SUCCESS (0) on success, or\r
+       an errorID.\r
+\r
+       You may optionally pass in the 'nBytes', which is\r
+       the number of bytes which will be parsed. If not\r
+       specified, TinyXML-2 will assume 'xml' points to a\r
+       null terminated string.\r
+    */\r
+    XMLError Parse( const char* xml, size_t nBytes=(size_t)(-1) );\r
+\r
+    /**\r
+       Load an XML file from disk.\r
+       Returns XML_SUCCESS (0) on success, or\r
+       an errorID.\r
+    */\r
+    XMLError LoadFile( const char* filename );\r
+\r
+    /**\r
+       Load an XML file from disk. You are responsible\r
+       for providing and closing the FILE*.\r
+\r
+        NOTE: The file should be opened as binary ("rb")\r
+        not text in order for TinyXML-2 to correctly\r
+        do newline normalization.\r
+\r
+       Returns XML_SUCCESS (0) on success, or\r
+       an errorID.\r
+    */\r
+    XMLError LoadFile( FILE* );\r
+\r
+    /**\r
+       Save the XML file to disk.\r
+       Returns XML_SUCCESS (0) on success, or\r
+       an errorID.\r
+    */\r
+    XMLError SaveFile( const char* filename, bool compact = false );\r
+\r
+    /**\r
+       Save the XML file to disk. You are responsible\r
+       for providing and closing the FILE*.\r
+\r
+       Returns XML_SUCCESS (0) on success, or\r
+       an errorID.\r
+    */\r
+    XMLError SaveFile( FILE* fp, bool compact = false );\r
+\r
+    bool ProcessEntities() const               {\r
+        return _processEntities;\r
+    }\r
+    Whitespace WhitespaceMode() const  {\r
+        return _whitespaceMode;\r
+    }\r
+\r
+    /**\r
+       Returns true if this document has a leading Byte Order Mark of UTF8.\r
+    */\r
+    bool HasBOM() const {\r
+        return _writeBOM;\r
+    }\r
+    /** Sets whether to write the BOM when writing the file.\r
+    */\r
+    void SetBOM( bool useBOM ) {\r
+        _writeBOM = useBOM;\r
+    }\r
+\r
+    /** Return the root element of DOM. Equivalent to FirstChildElement().\r
+        To get the first node, use FirstChild().\r
+    */\r
+    XMLElement* RootElement()                          {\r
+        return FirstChildElement();\r
+    }\r
+    const XMLElement* RootElement() const      {\r
+        return FirstChildElement();\r
+    }\r
+\r
+    /** Print the Document. If the Printer is not provided, it will\r
+        print to stdout. If you provide Printer, this can print to a file:\r
+       @verbatim\r
+       XMLPrinter printer( fp );\r
+       doc.Print( &printer );\r
+       @endverbatim\r
+\r
+       Or you can use a printer to print to memory:\r
+       @verbatim\r
+       XMLPrinter printer;\r
+       doc.Print( &printer );\r
+       // printer.CStr() has a const char* to the XML\r
+       @endverbatim\r
+    */\r
+    void Print( XMLPrinter* streamer=0 ) const;\r
+    virtual bool Accept( XMLVisitor* visitor ) const;\r
+\r
+    /**\r
+       Create a new Element associated with\r
+       this Document. The memory for the Element\r
+       is managed by the Document.\r
+    */\r
+    XMLElement* NewElement( const char* name );\r
+    /**\r
+       Create a new Comment associated with\r
+       this Document. The memory for the Comment\r
+       is managed by the Document.\r
+    */\r
+    XMLComment* NewComment( const char* comment );\r
+    /**\r
+       Create a new Text associated with\r
+       this Document. The memory for the Text\r
+       is managed by the Document.\r
+    */\r
+    XMLText* NewText( const char* text );\r
+    /**\r
+       Create a new Declaration associated with\r
+       this Document. The memory for the object\r
+       is managed by the Document.\r
+\r
+       If the 'text' param is null, the standard\r
+       declaration is used.:\r
+       @verbatim\r
+               <?xml version="1.0" encoding="UTF-8"?>\r
+       @endverbatim\r
+    */\r
+    XMLDeclaration* NewDeclaration( const char* text=0 );\r
+    /**\r
+       Create a new Unknown associated with\r
+       this Document. The memory for the object\r
+       is managed by the Document.\r
+    */\r
+    XMLUnknown* NewUnknown( const char* text );\r
+\r
+    /**\r
+       Delete a node associated with this document.\r
+       It will be unlinked from the DOM.\r
+    */\r
+    void DeleteNode( XMLNode* node );\r
+\r
+    void ClearError() {\r
+        SetError(XML_SUCCESS, 0, 0);\r
+    }\r
+\r
+    /// Return true if there was an error parsing the document.\r
+    bool Error() const {\r
+        return _errorID != XML_SUCCESS;\r
+    }\r
+    /// Return the errorID.\r
+    XMLError  ErrorID() const {\r
+        return _errorID;\r
+    }\r
+       const char* ErrorName() const;\r
+    static const char* ErrorIDToName(XMLError errorID);\r
+\r
+    /** Returns a "long form" error description. A hopefully helpful\r
+        diagnostic with location, line number, and/or additional info.\r
+    */\r
+       const char* ErrorStr() const;\r
+\r
+    /// A (trivial) utility function that prints the ErrorStr() to stdout.\r
+    void PrintError() const;\r
+\r
+    /// Return the line where the error occurred, or zero if unknown.\r
+    int ErrorLineNum() const\r
+    {\r
+        return _errorLineNum;\r
+    }\r
+\r
+    /// Clear the document, resetting it to the initial state.\r
+    void Clear();\r
+\r
+       /**\r
+               Copies this document to a target document.\r
+               The target will be completely cleared before the copy.\r
+               If you want to copy a sub-tree, see XMLNode::DeepClone().\r
+\r
+               NOTE: that the 'target' must be non-null.\r
+       */\r
+       void DeepCopy(XMLDocument* target) const;\r
+\r
+       // internal\r
+    char* Identify( char* p, XMLNode** node );\r
+\r
+       // internal\r
+       void MarkInUse(XMLNode*);\r
+\r
+    virtual XMLNode* ShallowClone( XMLDocument* /*document*/ ) const   {\r
+        return 0;\r
+    }\r
+    virtual bool ShallowEqual( const XMLNode* /*compare*/ ) const      {\r
+        return false;\r
+    }\r
+\r
+private:\r
+    XMLDocument( const XMLDocument& ); // not supported\r
+    void operator=( const XMLDocument& );      // not supported\r
+\r
+    bool                       _writeBOM;\r
+    bool                       _processEntities;\r
+    XMLError           _errorID;\r
+    Whitespace         _whitespaceMode;\r
+    mutable StrPair    _errorStr;\r
+    int             _errorLineNum;\r
+    char*                      _charBuffer;\r
+    int                                _parseCurLineNum;\r
+       int                             _parsingDepth;\r
+       // Memory tracking does add some overhead.\r
+       // However, the code assumes that you don't\r
+       // have a bunch of unlinked nodes around.\r
+       // Therefore it takes less memory to track\r
+       // in the document vs. a linked list in the XMLNode,\r
+       // and the performance is the same.\r
+       DynArray<XMLNode*, 10> _unlinked;\r
+\r
+    MemPoolT< sizeof(XMLElement) >      _elementPool;\r
+    MemPoolT< sizeof(XMLAttribute) > _attributePool;\r
+    MemPoolT< sizeof(XMLText) >                 _textPool;\r
+    MemPoolT< sizeof(XMLComment) >      _commentPool;\r
+\r
+       static const char* _errorNames[XML_ERROR_COUNT];\r
+\r
+    void Parse();\r
+\r
+    void SetError( XMLError error, int lineNum, const char* format, ... );\r
+\r
+       // Something of an obvious security hole, once it was discovered.\r
+       // Either an ill-formed XML or an excessively deep one can overflow\r
+       // the stack. Track stack depth, and error out if needed.\r
+       class DepthTracker {\r
+       public:\r
+               explicit DepthTracker(XMLDocument * document) {\r
+                       this->_document = document;\r
+                       document->PushDepth();\r
+               }\r
+               ~DepthTracker() {\r
+                       _document->PopDepth();\r
+               }\r
+       private:\r
+               XMLDocument * _document;\r
+       };\r
+       void PushDepth();\r
+       void PopDepth();\r
+\r
+    template<class NodeType, int PoolElementSize>\r
+    NodeType* CreateUnlinkedNode( MemPoolT<PoolElementSize>& pool );\r
+};\r
+\r
+template<class NodeType, int PoolElementSize>\r
+inline NodeType* XMLDocument::CreateUnlinkedNode( MemPoolT<PoolElementSize>& pool )\r
+{\r
+    TIXMLASSERT( sizeof( NodeType ) == PoolElementSize );\r
+    TIXMLASSERT( sizeof( NodeType ) == pool.ItemSize() );\r
+    NodeType* returnNode = new (pool.Alloc()) NodeType( this );\r
+    TIXMLASSERT( returnNode );\r
+    returnNode->_memPool = &pool;\r
+\r
+       _unlinked.Push(returnNode);\r
+    return returnNode;\r
+}\r
+\r
+/**\r
+       A XMLHandle is a class that wraps a node pointer with null checks; this is\r
+       an incredibly useful thing. Note that XMLHandle is not part of the TinyXML-2\r
+       DOM structure. It is a separate utility class.\r
+\r
+       Take an example:\r
+       @verbatim\r
+       <Document>\r
+               <Element attributeA = "valueA">\r
+                       <Child attributeB = "value1" />\r
+                       <Child attributeB = "value2" />\r
+               </Element>\r
+       </Document>\r
+       @endverbatim\r
+\r
+       Assuming you want the value of "attributeB" in the 2nd "Child" element, it's very\r
+       easy to write a *lot* of code that looks like:\r
+\r
+       @verbatim\r
+       XMLElement* root = document.FirstChildElement( "Document" );\r
+       if ( root )\r
+       {\r
+               XMLElement* element = root->FirstChildElement( "Element" );\r
+               if ( element )\r
+               {\r
+                       XMLElement* child = element->FirstChildElement( "Child" );\r
+                       if ( child )\r
+                       {\r
+                               XMLElement* child2 = child->NextSiblingElement( "Child" );\r
+                               if ( child2 )\r
+                               {\r
+                                       // Finally do something useful.\r
+       @endverbatim\r
+\r
+       And that doesn't even cover "else" cases. XMLHandle addresses the verbosity\r
+       of such code. A XMLHandle checks for null pointers so it is perfectly safe\r
+       and correct to use:\r
+\r
+       @verbatim\r
+       XMLHandle docHandle( &document );\r
+       XMLElement* child2 = docHandle.FirstChildElement( "Document" ).FirstChildElement( "Element" ).FirstChildElement().NextSiblingElement();\r
+       if ( child2 )\r
+       {\r
+               // do something useful\r
+       @endverbatim\r
+\r
+       Which is MUCH more concise and useful.\r
+\r
+       It is also safe to copy handles - internally they are nothing more than node pointers.\r
+       @verbatim\r
+       XMLHandle handleCopy = handle;\r
+       @endverbatim\r
+\r
+       See also XMLConstHandle, which is the same as XMLHandle, but operates on const objects.\r
+*/\r
+class TINYXML2_LIB XMLHandle\r
+{\r
+public:\r
+    /// Create a handle from any node (at any depth of the tree.) This can be a null pointer.\r
+    explicit XMLHandle( XMLNode* node ) : _node( node ) {\r
+    }\r
+    /// Create a handle from a node.\r
+    explicit XMLHandle( XMLNode& node ) : _node( &node ) {\r
+    }\r
+    /// Copy constructor\r
+    XMLHandle( const XMLHandle& ref ) : _node( ref._node ) {\r
+    }\r
+    /// Assignment\r
+    XMLHandle& operator=( const XMLHandle& ref )                                                       {\r
+        _node = ref._node;\r
+        return *this;\r
+    }\r
+\r
+    /// Get the first child of this handle.\r
+    XMLHandle FirstChild()                                                                                                     {\r
+        return XMLHandle( _node ? _node->FirstChild() : 0 );\r
+    }\r
+    /// Get the first child element of this handle.\r
+    XMLHandle FirstChildElement( const char* name = 0 )                                                {\r
+        return XMLHandle( _node ? _node->FirstChildElement( name ) : 0 );\r
+    }\r
+    /// Get the last child of this handle.\r
+    XMLHandle LastChild()                                                                                                      {\r
+        return XMLHandle( _node ? _node->LastChild() : 0 );\r
+    }\r
+    /// Get the last child element of this handle.\r
+    XMLHandle LastChildElement( const char* name = 0 )                                         {\r
+        return XMLHandle( _node ? _node->LastChildElement( name ) : 0 );\r
+    }\r
+    /// Get the previous sibling of this handle.\r
+    XMLHandle PreviousSibling()                                                                                                {\r
+        return XMLHandle( _node ? _node->PreviousSibling() : 0 );\r
+    }\r
+    /// Get the previous sibling element of this handle.\r
+    XMLHandle PreviousSiblingElement( const char* name = 0 )                           {\r
+        return XMLHandle( _node ? _node->PreviousSiblingElement( name ) : 0 );\r
+    }\r
+    /// Get the next sibling of this handle.\r
+    XMLHandle NextSibling()                                                                                                    {\r
+        return XMLHandle( _node ? _node->NextSibling() : 0 );\r
+    }\r
+    /// Get the next sibling element of this handle.\r
+    XMLHandle NextSiblingElement( const char* name = 0 )                                       {\r
+        return XMLHandle( _node ? _node->NextSiblingElement( name ) : 0 );\r
+    }\r
+\r
+    /// Safe cast to XMLNode. This can return null.\r
+    XMLNode* ToNode()                                                  {\r
+        return _node;\r
+    }\r
+    /// Safe cast to XMLElement. This can return null.\r
+    XMLElement* ToElement()                                    {\r
+        return ( _node ? _node->ToElement() : 0 );\r
+    }\r
+    /// Safe cast to XMLText. This can return null.\r
+    XMLText* ToText()                                                  {\r
+        return ( _node ? _node->ToText() : 0 );\r
+    }\r
+    /// Safe cast to XMLUnknown. This can return null.\r
+    XMLUnknown* ToUnknown()                                    {\r
+        return ( _node ? _node->ToUnknown() : 0 );\r
+    }\r
+    /// Safe cast to XMLDeclaration. This can return null.\r
+    XMLDeclaration* ToDeclaration()                    {\r
+        return ( _node ? _node->ToDeclaration() : 0 );\r
+    }\r
+\r
+private:\r
+    XMLNode* _node;\r
+};\r
+\r
+\r
+/**\r
+       A variant of the XMLHandle class for working with const XMLNodes and Documents. It is the\r
+       same in all regards, except for the 'const' qualifiers. See XMLHandle for API.\r
+*/\r
+class TINYXML2_LIB XMLConstHandle\r
+{\r
+public:\r
+    explicit XMLConstHandle( const XMLNode* node ) : _node( node ) {\r
+    }\r
+    explicit XMLConstHandle( const XMLNode& node ) : _node( &node ) {\r
+    }\r
+    XMLConstHandle( const XMLConstHandle& ref ) : _node( ref._node ) {\r
+    }\r
+\r
+    XMLConstHandle& operator=( const XMLConstHandle& ref )                                                     {\r
+        _node = ref._node;\r
+        return *this;\r
+    }\r
+\r
+    const XMLConstHandle FirstChild() const                                                                                    {\r
+        return XMLConstHandle( _node ? _node->FirstChild() : 0 );\r
+    }\r
+    const XMLConstHandle FirstChildElement( const char* name = 0 ) const                               {\r
+        return XMLConstHandle( _node ? _node->FirstChildElement( name ) : 0 );\r
+    }\r
+    const XMLConstHandle LastChild()   const                                                                           {\r
+        return XMLConstHandle( _node ? _node->LastChild() : 0 );\r
+    }\r
+    const XMLConstHandle LastChildElement( const char* name = 0 ) const                                {\r
+        return XMLConstHandle( _node ? _node->LastChildElement( name ) : 0 );\r
+    }\r
+    const XMLConstHandle PreviousSibling() const                                                                       {\r
+        return XMLConstHandle( _node ? _node->PreviousSibling() : 0 );\r
+    }\r
+    const XMLConstHandle PreviousSiblingElement( const char* name = 0 ) const          {\r
+        return XMLConstHandle( _node ? _node->PreviousSiblingElement( name ) : 0 );\r
+    }\r
+    const XMLConstHandle NextSibling() const                                                                           {\r
+        return XMLConstHandle( _node ? _node->NextSibling() : 0 );\r
+    }\r
+    const XMLConstHandle NextSiblingElement( const char* name = 0 ) const                      {\r
+        return XMLConstHandle( _node ? _node->NextSiblingElement( name ) : 0 );\r
+    }\r
+\r
+\r
+    const XMLNode* ToNode() const                              {\r
+        return _node;\r
+    }\r
+    const XMLElement* ToElement() const                        {\r
+        return ( _node ? _node->ToElement() : 0 );\r
+    }\r
+    const XMLText* ToText() const                              {\r
+        return ( _node ? _node->ToText() : 0 );\r
+    }\r
+    const XMLUnknown* ToUnknown() const                        {\r
+        return ( _node ? _node->ToUnknown() : 0 );\r
+    }\r
+    const XMLDeclaration* ToDeclaration() const        {\r
+        return ( _node ? _node->ToDeclaration() : 0 );\r
+    }\r
+\r
+private:\r
+    const XMLNode* _node;\r
+};\r
+\r
+\r
+/**\r
+       Printing functionality. The XMLPrinter gives you more\r
+       options than the XMLDocument::Print() method.\r
+\r
+       It can:\r
+       -# Print to memory.\r
+       -# Print to a file you provide.\r
+       -# Print XML without a XMLDocument.\r
+\r
+       Print to Memory\r
+\r
+       @verbatim\r
+       XMLPrinter printer;\r
+       doc.Print( &printer );\r
+       SomeFunction( printer.CStr() );\r
+       @endverbatim\r
+\r
+       Print to a File\r
+\r
+       You provide the file pointer.\r
+       @verbatim\r
+       XMLPrinter printer( fp );\r
+       doc.Print( &printer );\r
+       @endverbatim\r
+\r
+       Print without a XMLDocument\r
+\r
+       When loading, an XML parser is very useful. However, sometimes\r
+       when saving, it just gets in the way. The code is often set up\r
+       for streaming, and constructing the DOM is just overhead.\r
+\r
+       The Printer supports the streaming case. The following code\r
+       prints out a trivially simple XML file without ever creating\r
+       an XML document.\r
+\r
+       @verbatim\r
+       XMLPrinter printer( fp );\r
+       printer.OpenElement( "foo" );\r
+       printer.PushAttribute( "foo", "bar" );\r
+       printer.CloseElement();\r
+       @endverbatim\r
+*/\r
+class TINYXML2_LIB XMLPrinter : public XMLVisitor\r
+{\r
+public:\r
+    /** Construct the printer. If the FILE* is specified,\r
+       this will print to the FILE. Else it will print\r
+       to memory, and the result is available in CStr().\r
+       If 'compact' is set to true, then output is created\r
+       with only required whitespace and newlines.\r
+    */\r
+    XMLPrinter( FILE* file=0, bool compact = false, int depth = 0 );\r
+    virtual ~XMLPrinter()      {}\r
+\r
+    /** If streaming, write the BOM and declaration. */\r
+    void PushHeader( bool writeBOM, bool writeDeclaration );\r
+    /** If streaming, start writing an element.\r
+        The element must be closed with CloseElement()\r
+    */\r
+    void OpenElement( const char* name, bool compactMode=false );\r
+    /// If streaming, add an attribute to an open element.\r
+    void PushAttribute( const char* name, const char* value );\r
+    void PushAttribute( const char* name, int value );\r
+    void PushAttribute( const char* name, unsigned value );\r
+       void PushAttribute(const char* name, int64_t value);\r
+       void PushAttribute( const char* name, bool value );\r
+    void PushAttribute( const char* name, double value );\r
+    /// If streaming, close the Element.\r
+    virtual void CloseElement( bool compactMode=false );\r
+\r
+    /// Add a text node.\r
+    void PushText( const char* text, bool cdata=false );\r
+    /// Add a text node from an integer.\r
+    void PushText( int value );\r
+    /// Add a text node from an unsigned.\r
+    void PushText( unsigned value );\r
+       /// Add a text node from an unsigned.\r
+       void PushText(int64_t value);\r
+       /// Add a text node from a bool.\r
+    void PushText( bool value );\r
+    /// Add a text node from a float.\r
+    void PushText( float value );\r
+    /// Add a text node from a double.\r
+    void PushText( double value );\r
+\r
+    /// Add a comment\r
+    void PushComment( const char* comment );\r
+\r
+    void PushDeclaration( const char* value );\r
+    void PushUnknown( const char* value );\r
+\r
+    virtual bool VisitEnter( const XMLDocument& /*doc*/ );\r
+    virtual bool VisitExit( const XMLDocument& /*doc*/ )                       {\r
+        return true;\r
+    }\r
+\r
+    virtual bool VisitEnter( const XMLElement& element, const XMLAttribute* attribute );\r
+    virtual bool VisitExit( const XMLElement& element );\r
+\r
+    virtual bool Visit( const XMLText& text );\r
+    virtual bool Visit( const XMLComment& comment );\r
+    virtual bool Visit( const XMLDeclaration& declaration );\r
+    virtual bool Visit( const XMLUnknown& unknown );\r
+\r
+    /**\r
+       If in print to memory mode, return a pointer to\r
+       the XML file in memory.\r
+    */\r
+    const char* CStr() const {\r
+        return _buffer.Mem();\r
+    }\r
+    /**\r
+       If in print to memory mode, return the size\r
+       of the XML file in memory. (Note the size returned\r
+       includes the terminating null.)\r
+    */\r
+    int CStrSize() const {\r
+        return _buffer.Size();\r
+    }\r
+    /**\r
+       If in print to memory mode, reset the buffer to the\r
+       beginning.\r
+    */\r
+    void ClearBuffer() {\r
+        _buffer.Clear();\r
+        _buffer.Push(0);\r
+               _firstElement = true;\r
+    }\r
+\r
+protected:\r
+       virtual bool CompactMode( const XMLElement& )   { return _compactMode; }\r
+\r
+       /** Prints out the space before an element. You may override to change\r
+           the space and tabs used. A PrintSpace() override should call Print().\r
+       */\r
+    virtual void PrintSpace( int depth );\r
+    void Print( const char* format, ... );\r
+    void Write( const char* data, size_t size );\r
+    inline void Write( const char* data )           { Write( data, strlen( data ) ); }\r
+    void Putc( char ch );\r
+\r
+    void SealElementIfJustOpened();\r
+    bool _elementJustOpened;\r
+    DynArray< const char*, 10 > _stack;\r
+\r
+private:\r
+    void PrintString( const char*, bool restrictedEntitySet ); // prints out, after detecting entities.\r
+\r
+    bool _firstElement;\r
+    FILE* _fp;\r
+    int _depth;\r
+    int _textDepth;\r
+    bool _processEntities;\r
+       bool _compactMode;\r
+\r
+    enum {\r
+        ENTITY_RANGE = 64,\r
+        BUF_SIZE = 200\r
+    };\r
+    bool _entityFlag[ENTITY_RANGE];\r
+    bool _restrictedEntityFlag[ENTITY_RANGE];\r
+\r
+    DynArray< char, 20 > _buffer;\r
+\r
+    // Prohibit cloning, intentionally not implemented\r
+    XMLPrinter( const XMLPrinter& );\r
+    XMLPrinter& operator=( const XMLPrinter& );\r
+};\r
+\r
+\r
+}      // tinyxml2\r
+\r
+#if defined(_MSC_VER)\r
+#   pragma warning(pop)\r
+#endif\r
+\r
+#endif // BT_TINYXML2_INCLUDED\r
diff --git a/src/shared_library.cpp b/src/shared_library.cpp
new file mode 100644 (file)
index 0000000..2e37b24
--- /dev/null
@@ -0,0 +1,26 @@
+#include "behaviortree_cpp_v3/utils/shared_library.h"
+#include "behaviortree_cpp_v3/exceptions.h"
+
+BT::SharedLibrary::SharedLibrary(const std::string& path, int flags)
+{
+    load(path, flags);
+}
+
+void* BT::SharedLibrary::getSymbol(const std::string& name)
+{
+    void* result = findSymbol(name);
+    if (result)
+        return result;
+    else
+        throw RuntimeError( "[SharedLibrary::getSymbol]: can't find symbol ", name );
+}
+
+bool BT::SharedLibrary::hasSymbol(const std::string& name)
+{
+    return findSymbol(name) != nullptr;
+}
+
+std::string BT::SharedLibrary::getOSName(const std::string& name)
+{
+    return prefix() + name + suffix();
+}
diff --git a/src/shared_library_UNIX.cpp b/src/shared_library_UNIX.cpp
new file mode 100644 (file)
index 0000000..7995ba4
--- /dev/null
@@ -0,0 +1,103 @@
+#include <string>
+#include <mutex>
+#include <dlfcn.h>
+#include "behaviortree_cpp_v3/utils/shared_library.h"
+#include "behaviortree_cpp_v3/exceptions.h"
+
+namespace BT
+{
+SharedLibrary::SharedLibrary()
+{
+    _handle = nullptr;
+}
+
+void SharedLibrary::load(const std::string& path, int)
+{
+    std::unique_lock<std::mutex> lock(_mutex);
+
+    if (_handle)
+    {
+        throw RuntimeError("Library already loaded: " + path);
+    }
+
+    _handle = dlopen(path.c_str(), RTLD_NOW | RTLD_GLOBAL);
+    if (!_handle)
+    {
+        const char* err = dlerror();
+        throw RuntimeError("Could not load library: " + (err ? std::string(err) : path));
+    }
+    _path = path;
+}
+
+void SharedLibrary::unload()
+{
+    std::unique_lock<std::mutex> lock(_mutex);
+
+    if (_handle)
+    {
+        dlclose(_handle);
+        _handle = nullptr;
+    }
+}
+
+bool SharedLibrary::isLoaded() const
+{
+    return _handle != nullptr;
+}
+
+void* SharedLibrary::findSymbol(const std::string& name)
+{
+    std::unique_lock<std::mutex> lock(_mutex);
+
+    void* result = nullptr;
+    if (_handle)
+    {
+        result = dlsym(_handle, name.c_str());
+    }
+    return result;
+}
+
+const std::string& SharedLibrary::getPath() const
+{
+    return _path;
+}
+
+std::string SharedLibrary::prefix()
+{
+#if BT_OS == BT_OS_CYGWIN
+    return "cyg";
+#else
+    return "lib";
+#endif
+}
+
+std::string SharedLibrary::suffix()
+{
+#if BT_OS == BT_OS_MAC_OS_X
+#if defined(_DEBUG) && !defined(CL_NO_SHARED_LIBRARY_DEBUG_SUFFIX)
+    return "d.dylib";
+#else
+    return ".dylib";
+#endif
+#elif BT_OS == BT_OS_HPUX
+#if defined(_DEBUG) && !defined(CL_NO_SHARED_LIBRARY_DEBUG_SUFFIX)
+    return "d.sl";
+#else
+    return ".sl";
+#endif
+#elif BT_OS == BT_OS_CYGWIN
+#if defined(_DEBUG) && !defined(CL_NO_SHARED_LIBRARY_DEBUG_SUFFIX)
+    return "d.dll";
+#else
+    return ".dll";
+#endif
+#else
+#if defined(_DEBUG) && !defined(CL_NO_SHARED_LIBRARY_DEBUG_SUFFIX)
+    return "d.so";
+#else
+    return ".so";
+#endif
+#endif
+}
+
+}   // namespace
diff --git a/src/shared_library_WIN.cpp b/src/shared_library_WIN.cpp
new file mode 100644 (file)
index 0000000..4a1809b
--- /dev/null
@@ -0,0 +1,80 @@
+#include <string>
+#include <mutex>
+#include <Windows.h>
+#include "behaviortree_cpp_v3/utils/shared_library.h"
+#include "behaviortree_cpp_v3/exceptions.h"
+
+namespace BT
+{
+SharedLibrary::SharedLibrary()
+{
+    _handle = nullptr;
+}
+
+void SharedLibrary::load(const std::string& path, int)
+{
+    std::unique_lock<std::mutex> lock(_mutex);
+
+    _handle = LoadLibrary(path.c_str());
+    if (!_handle)
+    {
+        throw RuntimeError("Could not load library: " + path);
+    }
+    _path = path;
+}
+
+void SharedLibrary::unload()
+{
+    std::unique_lock<std::mutex> lock(_mutex);
+
+    if (_handle)
+    {
+        FreeLibrary((HMODULE)_handle);
+        _handle = 0;
+    }
+    _path.clear();
+}
+
+bool SharedLibrary::isLoaded() const
+{
+    return _handle != nullptr;
+}
+
+void* SharedLibrary::findSymbol(const std::string& name)
+{
+    std::unique_lock<std::mutex> lock(_mutex);
+
+    if (_handle)
+    {
+#if defined(_WIN32_WCE)
+        std::wstring uname;
+        UnicodeConverter::toUTF16(name, uname);
+        return (void*)GetProcAddressW((HMODULE)_handle, uname.c_str());
+#else
+        return (void*)GetProcAddress((HMODULE)_handle, name.c_str());
+#endif
+    }
+
+    return 0;
+}
+
+const std::string& SharedLibrary::getPath() const
+{
+    return _path;
+}
+
+std::string SharedLibrary::prefix()
+{
+    return "";
+}
+
+std::string SharedLibrary::suffix()
+{
+#if defined(_DEBUG)
+    return "d.dll";
+#else
+    return ".dll";
+#endif
+}
+
+}   // namespace BT
diff --git a/src/tree_node.cpp b/src/tree_node.cpp
new file mode 100644 (file)
index 0000000..7b71f46
--- /dev/null
@@ -0,0 +1,177 @@
+/* Copyright (C) 2015-2018 Michele Colledanchise -  All Rights Reserved
+ * Copyright (C) 2018-2020 Davide Faconti, Eurecat -  All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#include "behaviortree_cpp_v3/tree_node.h"
+#include <cstring>
+
+namespace BT
+{
+static uint16_t getUID()
+{
+    static uint16_t uid = 1;
+    return uid++;
+}
+
+TreeNode::TreeNode(std::string name, NodeConfiguration config)
+  : name_(std::move(name)),
+    status_(NodeStatus::IDLE),
+    uid_(getUID()),
+    config_(std::move(config))
+{
+}
+
+NodeStatus TreeNode::executeTick()
+{
+    const NodeStatus status = tick();
+    setStatus(status);
+    return status;
+}
+
+void TreeNode::setStatus(NodeStatus new_status)
+{
+    NodeStatus prev_status;
+    {
+        std::unique_lock<std::mutex> UniqueLock(state_mutex_);
+        prev_status = status_;
+        status_ = new_status;
+    }
+    if (prev_status != new_status)
+    {
+        state_condition_variable_.notify_all();
+        state_change_signal_.notify(std::chrono::high_resolution_clock::now(), *this, prev_status,
+                                    new_status);
+    }
+}
+
+NodeStatus TreeNode::status() const
+{
+    std::lock_guard<std::mutex> lock(state_mutex_);
+    return status_;
+}
+
+NodeStatus TreeNode::waitValidStatus()
+{
+    std::unique_lock<std::mutex> lock(state_mutex_);
+
+    while( isHalted() )
+    {
+        state_condition_variable_.wait(lock);
+    }
+    return status_;
+}
+
+const std::string& TreeNode::name() const
+{
+    return name_;
+}
+
+bool TreeNode::isHalted() const
+{
+    return status_ == NodeStatus::IDLE;
+}
+
+TreeNode::StatusChangeSubscriber
+TreeNode::subscribeToStatusChange(TreeNode::StatusChangeCallback callback)
+{
+    return state_change_signal_.subscribe(std::move(callback));
+}
+
+uint16_t TreeNode::UID() const
+{
+    return uid_;
+}
+
+const std::string& TreeNode::registrationName() const
+{
+    return registration_ID_;
+}
+
+const NodeConfiguration &TreeNode::config() const
+{
+    return config_;
+}
+
+StringView TreeNode::getRawPortValue(const std::string& key) const
+{
+  auto remap_it = config_.input_ports.find(key);
+  if (remap_it == config_.input_ports.end())
+  {
+    throw std::logic_error(StrCat("getInput() failed because "
+      "NodeConfiguration::input_ports "
+      "does not contain the key: [",
+      key, "]"));
+  }
+  return remap_it->second;
+}
+
+bool TreeNode::isBlackboardPointer(StringView str)
+{
+    const auto size = str.size();
+    if( size >= 3 && str.back() == '}')
+    {
+        if( str[0] == '{') {
+            return true;
+        }
+        if( size >= 4 && str[0] == '$' && str[1] == '{') {
+            return true;
+        }
+    }
+    return false;
+}
+
+StringView TreeNode::stripBlackboardPointer(StringView str)
+{
+    const auto size = str.size();
+    if( size >= 3 && str.back() == '}')
+    {
+        if( str[0] == '{') {
+            return str.substr(1, size-2);
+        }
+        if( str[0] == '$' && str[1] == '{') {
+            return str.substr(2, size-3);
+        }
+    }
+    return {};
+}
+
+Optional<StringView> TreeNode::getRemappedKey(StringView port_name, StringView remapping_value)
+{
+    if( remapping_value == "=" )
+    {
+        return {port_name};
+    }
+    if( isBlackboardPointer( remapping_value ) )
+    {
+        return {stripBlackboardPointer(remapping_value)};
+    }
+    return nonstd::make_unexpected("Not a blackboard pointer");
+}
+
+void TreeNode::modifyPortsRemapping(const PortsRemapping &new_remapping)
+{
+    for (const auto& new_it: new_remapping)
+    {
+        auto it = config_.input_ports.find( new_it.first );
+        if( it != config_.input_ports.end() )
+        {
+            it->second = new_it.second;
+        }
+        it = config_.output_ports.find( new_it.first );
+        if( it != config_.output_ports.end() )
+        {
+            it->second = new_it.second;
+        }
+    }
+}
+
+}   // end namespace
diff --git a/src/xml_parsing.cpp b/src/xml_parsing.cpp
new file mode 100644 (file)
index 0000000..62d0e45
--- /dev/null
@@ -0,0 +1,811 @@
+/*  Copyright (C) 2018-2020 Davide Faconti, Eurecat -  All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#include <functional>
+#include <list>
+
+#if defined(__linux) || defined(__linux__)
+  #pragma GCC diagnostic push
+  #pragma GCC diagnostic ignored "-Wattributes"
+#endif
+
+#ifdef _MSC_VER
+#pragma warning(disable : 4996) // do not complain about sprintf
+#endif
+
+#include "behaviortree_cpp_v3/xml_parsing.h"
+#include "private/tinyxml2.h"
+#include "filesystem/path.h"
+
+#ifdef USING_ROS
+#include <ros/package.h>
+#endif
+
+#include "behaviortree_cpp_v3/blackboard.h"
+#include "behaviortree_cpp_v3/utils/demangle_util.h"
+
+namespace BT
+{
+using namespace BT_TinyXML2;
+
+struct XMLParser::Pimpl
+{
+    TreeNode::Ptr createNodeFromXML(const XMLElement* element,
+                                    const Blackboard::Ptr& blackboard,
+                                    const TreeNode::Ptr& node_parent);
+
+    void recursivelyCreateTree(const std::string& tree_ID,
+                               Tree& output_tree,
+                               Blackboard::Ptr blackboard,
+                               const TreeNode::Ptr& root_parent);
+
+    void loadDocImpl(BT_TinyXML2::XMLDocument* doc);
+
+    std::list<std::unique_ptr<BT_TinyXML2::XMLDocument> > opened_documents;
+    std::unordered_map<std::string,const XMLElement*>  tree_roots;
+
+    const BehaviorTreeFactory& factory;
+
+    filesystem::path current_path;
+
+    int suffix_count;
+
+    explicit Pimpl(const BehaviorTreeFactory &fact):
+        factory(fact),
+        current_path( filesystem::path::getcwd() ),
+        suffix_count(0)
+    {}
+
+    void clear()
+    {
+        suffix_count = 0;
+        current_path = filesystem::path::getcwd();
+        opened_documents.clear();
+        tree_roots.clear();
+    }
+
+};
+
+#if defined(__linux) || defined(__linux__)
+#pragma GCC diagnostic pop
+#endif
+
+XMLParser::XMLParser(const BehaviorTreeFactory &factory) :
+    _p( new Pimpl(factory) )
+{
+}
+
+XMLParser::~XMLParser()
+{
+    delete _p;
+}
+
+void XMLParser::loadFromFile(const std::string& filename)
+{
+    _p->opened_documents.emplace_back(new BT_TinyXML2::XMLDocument());
+
+    BT_TinyXML2::XMLDocument* doc = _p->opened_documents.back().get();
+    doc->LoadFile(filename.c_str());
+
+    filesystem::path file_path( filename );
+    _p->current_path = file_path.parent_path().make_absolute();
+
+    _p->loadDocImpl( doc );
+}
+
+void XMLParser::loadFromText(const std::string& xml_text)
+{
+    _p->opened_documents.emplace_back(new BT_TinyXML2::XMLDocument());
+
+    BT_TinyXML2::XMLDocument* doc = _p->opened_documents.back().get();
+    doc->Parse(xml_text.c_str(), xml_text.size());
+
+    _p->loadDocImpl( doc );
+}
+
+void XMLParser::Pimpl::loadDocImpl(BT_TinyXML2::XMLDocument* doc)
+{
+    if (doc->Error())
+    {
+        char buffer[200];
+        sprintf(buffer, "Error parsing the XML: %s", doc->ErrorName() );
+        throw RuntimeError(buffer);
+    }
+
+    const XMLElement* xml_root = doc->RootElement();
+
+    // recursively include other files
+    for (auto include_node = xml_root->FirstChildElement("include");
+         include_node != nullptr;
+         include_node = include_node->NextSiblingElement("include"))
+    {
+
+        filesystem::path file_path( include_node->Attribute("path") );
+
+        if( include_node->Attribute("ros_pkg") )
+        {
+#ifdef USING_ROS
+            if( file_path.is_absolute() )
+            {
+                std::cout << "WARNING: <include path=\"...\"> containes an absolute path.\n"
+                          << "Attribute [ros_pkg] will be ignored."<< std::endl;
+            }
+            else {
+                auto ros_pkg_path = ros::package::getPath(  include_node->Attribute("ros_pkg") );
+                file_path = filesystem::path( ros_pkg_path ) / file_path;
+            }
+#else
+            throw RuntimeError("Using attribute [ros_pkg] in <include>, but this library was compiled "
+                               "without ROS support. Recompile the BehaviorTree.CPP using catkin");
+#endif
+        }
+
+        if( !file_path.is_absolute() )
+        {
+            file_path = current_path / file_path;
+        }
+
+        opened_documents.emplace_back(new BT_TinyXML2::XMLDocument());
+        BT_TinyXML2::XMLDocument* next_doc = opened_documents.back().get();
+        next_doc->LoadFile(file_path.str().c_str());
+        loadDocImpl(next_doc);
+    }
+
+    for (auto bt_node = xml_root->FirstChildElement("BehaviorTree");
+         bt_node != nullptr;
+         bt_node = bt_node->NextSiblingElement("BehaviorTree"))
+    {
+        std::string tree_name;
+        if (bt_node->Attribute("ID"))
+        {
+            tree_name = bt_node->Attribute("ID");
+        }
+        else{
+            tree_name = "BehaviorTree_" + std::to_string( suffix_count++ );
+        }
+        tree_roots.insert( {tree_name, bt_node} );
+    }
+
+    std::set<std::string> registered_nodes;
+    XMLPrinter printer;
+    doc->Print(&printer);
+    auto xml_text = std::string(printer.CStr(), size_t(printer.CStrSize() - 1));
+
+    for( const auto& it: factory.manifests())
+    {
+        registered_nodes.insert( it.first );
+    }
+    for( const auto& it: tree_roots)
+    {
+        registered_nodes.insert( it.first );
+    }
+
+    VerifyXML(xml_text, registered_nodes);
+}
+
+void VerifyXML(const std::string& xml_text,
+               const std::set<std::string>& registered_nodes)
+{
+
+    BT_TinyXML2::XMLDocument doc;
+    auto xml_error = doc.Parse( xml_text.c_str(), xml_text.size());
+    if (xml_error)
+    {
+        char buffer[200];
+        sprintf(buffer, "Error parsing the XML: %s", doc.ErrorName() );
+        throw RuntimeError( buffer );
+    }
+
+    //-------- Helper functions (lambdas) -----------------
+    auto StrEqual = [](const char* str1, const char* str2) -> bool {
+        return strcmp(str1, str2) == 0;
+    };
+
+    auto ThrowError = [&](int line_num, const std::string& text) {
+        char buffer[256];
+        sprintf(buffer, "Error at line %d: -> %s", line_num, text.c_str());
+        throw RuntimeError( buffer );
+    };
+
+    auto ChildrenCount = [](const XMLElement* parent_node) {
+        int count = 0;
+        for (auto node = parent_node->FirstChildElement(); node != nullptr;
+             node = node->NextSiblingElement())
+        {
+            count++;
+        }
+        return count;
+    };
+    //-----------------------------
+
+    const XMLElement* xml_root = doc.RootElement();
+
+    if (!xml_root || !StrEqual(xml_root->Name(), "root"))
+    {
+        throw RuntimeError("The XML must have a root node called <root>");
+    }
+    //-------------------------------------------------
+    auto models_root = xml_root->FirstChildElement("TreeNodesModel");
+    auto meta_sibling = models_root ? models_root->NextSiblingElement("TreeNodesModel") : nullptr;
+
+    if (meta_sibling)
+    {
+       ThrowError(meta_sibling->GetLineNum(),
+                           " Only a single node <TreeNodesModel> is supported");
+    }
+    if (models_root)
+    {
+        // not having a MetaModel is not an error. But consider that the
+        // Graphical editor needs it.
+        for (auto node = xml_root->FirstChildElement(); node != nullptr;
+             node = node->NextSiblingElement())
+        {
+            const char* name = node->Name();
+            if (StrEqual(name, "Action") || StrEqual(name, "Decorator") ||
+                    StrEqual(name, "SubTree") || StrEqual(name, "Condition") || StrEqual(name, "Control"))
+            {
+                const char* ID = node->Attribute("ID");
+                if (!ID)
+                {
+                   ThrowError(node->GetLineNum(),
+                                       "Error at line %d: -> The attribute [ID] is mandatory");
+                }
+            }
+        }
+    }
+    //-------------------------------------------------
+
+    // function to be called recursively
+    std::function<void(const XMLElement*)> recursiveStep;
+
+    recursiveStep = [&](const XMLElement* node) {
+        const int children_count = ChildrenCount(node);
+        const char* name = node->Name();
+        if (StrEqual(name, "Decorator"))
+        {
+            if (children_count != 1)
+            {
+               ThrowError(node->GetLineNum(),
+                                   "The node <Decorator> must have exactly 1 child");
+            }
+            if (!node->Attribute("ID"))
+            {
+               ThrowError(node->GetLineNum(),
+                                   "The node <Decorator> must have the attribute [ID]");
+            }
+        }
+        else if (StrEqual(name, "Action"))
+        {
+            if (children_count != 0)
+            {
+               ThrowError(node->GetLineNum(),
+                                   "The node <Action> must not have any child");
+            }
+            if (!node->Attribute("ID"))
+            {
+               ThrowError(node->GetLineNum(),
+                                   "The node <Action> must have the attribute [ID]");
+            }
+        }
+        else if (StrEqual(name, "Condition"))
+        {
+            if (children_count != 0)
+            {
+               ThrowError(node->GetLineNum(),
+                                   "The node <Condition> must not have any child");
+            }
+            if (!node->Attribute("ID"))
+            {
+               ThrowError(node->GetLineNum(),
+                                   "The node <Condition> must have the attribute [ID]");
+            }
+        }
+        else if (StrEqual(name, "Control"))
+        {
+            if (children_count == 0)
+            {
+               ThrowError(node->GetLineNum(),
+                                   "The node <Control> must have at least 1 child");
+            }
+            if (!node->Attribute("ID"))
+            {
+               ThrowError(node->GetLineNum(),
+                                   "The node <Control> must have the attribute [ID]");
+            }
+        }
+        else if (StrEqual(name, "Sequence") ||
+                 StrEqual(name, "SequenceStar") ||
+                 StrEqual(name, "Fallback") )
+        {
+            if (children_count == 0)
+            {
+               ThrowError(node->GetLineNum(),
+                                   "A Control node must have at least 1 child");
+            }
+        }
+        else if (StrEqual(name, "SubTree"))
+        {
+            auto child = node->FirstChildElement();
+
+            if (child)
+            {
+                if (StrEqual(child->Name(), "remap"))
+                {
+                    ThrowError(node->GetLineNum(), "<remap> was deprecated");
+                }
+                else{
+                    ThrowError(node->GetLineNum(), "<SubTree> should not have any child");
+                }
+            }
+
+            if (!node->Attribute("ID"))
+            {
+               ThrowError(node->GetLineNum(),
+                                   "The node <SubTree> must have the attribute [ID]");
+            }
+        }
+        else
+        {
+            // search in the factory and the list of subtrees
+            bool found = ( registered_nodes.find(name)  != registered_nodes.end() );
+            if (!found)
+            {
+               ThrowError(node->GetLineNum(),
+                          std::string("Node not recognized: ") + name);
+            }
+        }
+        //recursion
+        if (StrEqual(name, "SubTree") == false)
+        {
+            for (auto child = node->FirstChildElement(); child != nullptr;
+                 child = child->NextSiblingElement())
+            {
+                recursiveStep(child);
+            }
+        }
+    };
+
+    std::vector<std::string> tree_names;
+    int tree_count = 0;
+
+    for (auto bt_root = xml_root->FirstChildElement("BehaviorTree"); bt_root != nullptr;
+         bt_root = bt_root->NextSiblingElement("BehaviorTree"))
+    {
+        tree_count++;
+        if (bt_root->Attribute("ID"))
+        {
+            tree_names.emplace_back(bt_root->Attribute("ID"));
+        }
+        if (ChildrenCount(bt_root) != 1)
+        {
+           ThrowError(bt_root->GetLineNum(),
+                               "The node <BehaviorTree> must have exactly 1 child");
+        }
+        else
+        {
+            recursiveStep(bt_root->FirstChildElement());
+        }
+    }
+
+    if (xml_root->Attribute("main_tree_to_execute"))
+    {
+        std::string main_tree = xml_root->Attribute("main_tree_to_execute");
+        if (std::find(tree_names.begin(), tree_names.end(), main_tree) == tree_names.end())
+        {
+            throw RuntimeError("The tree specified in [main_tree_to_execute] can't be found");
+        }
+    }
+    else
+    {
+        if (tree_count != 1)
+        {
+           throw RuntimeError("If you don't specify the attribute [main_tree_to_execute], "
+                              "Your file must contain a single BehaviorTree");
+        }
+    }
+}
+
+Tree XMLParser::instantiateTree(const Blackboard::Ptr& root_blackboard)
+{
+    Tree output_tree;
+
+    XMLElement* xml_root = _p->opened_documents.front()->RootElement();
+
+    std::string main_tree_ID;
+    if (xml_root->Attribute("main_tree_to_execute"))
+    {
+        main_tree_ID = xml_root->Attribute("main_tree_to_execute");
+    }
+    else if( _p->tree_roots.size() == 1)
+    {
+        main_tree_ID = _p->tree_roots.begin()->first;
+    }
+    else{
+        throw RuntimeError("[main_tree_to_execute] was not specified correctly");
+    }
+    //--------------------------------------
+    if( !root_blackboard )
+    {
+        throw RuntimeError("XMLParser::instantiateTree needs a non-empty root_blackboard");
+    }
+    // first blackboard
+    output_tree.blackboard_stack.push_back( root_blackboard );
+
+    _p->recursivelyCreateTree(main_tree_ID,
+                              output_tree,
+                              root_blackboard,
+                              TreeNode::Ptr() );
+    return output_tree;
+}
+
+TreeNode::Ptr XMLParser::Pimpl::createNodeFromXML(const XMLElement *element,
+                                                  const Blackboard::Ptr &blackboard,
+                                                  const TreeNode::Ptr &node_parent)
+{
+    const std::string element_name = element->Name();
+    std::string ID;
+    std::string instance_name;
+
+    // Actions and Decorators have their own ID
+    if (element_name == "Action" || element_name == "Decorator" ||
+        element_name == "Condition" || element_name == "Control")
+    {
+        ID = element->Attribute("ID");
+    }
+    else
+    {
+        ID = element_name;
+    }
+
+    const char* attr_alias = element->Attribute("name");
+    if (attr_alias)
+    {
+        instance_name = attr_alias;
+    }
+    else
+    {
+        instance_name = ID;
+    }
+
+    PortsRemapping port_remap;
+
+    if (element_name == "SubTree" ||
+        element_name == "SubTreePlus" )
+    {
+        instance_name = element->Attribute("ID");
+    }
+    else{
+        // do this only if it NOT a Subtree
+        for (const XMLAttribute* att = element->FirstAttribute(); att; att = att->Next())
+        {
+            const std::string attribute_name = att->Name();
+            if (attribute_name != "ID" && attribute_name != "name")
+            {
+                port_remap[attribute_name] = att->Value();
+            }
+        }
+    }
+    NodeConfiguration config;
+    config.blackboard = blackboard;
+
+    //---------------------------------------------
+    TreeNode::Ptr child_node;
+
+    if( factory.builders().count(ID) != 0)
+    {
+        const auto& manifest = factory.manifests().at(ID);
+
+        //Check that name in remapping can be found in the manifest
+        for(const auto& remap_it: port_remap)
+        {
+            if( manifest.ports.count( remap_it.first ) == 0 )
+            {
+                throw RuntimeError("Possible typo? In the XML, you tried to remap port \"",
+                                   remap_it.first, "\" in node [", ID," / ", instance_name,
+                                   "], but the manifest of this node does not contain a port with this name.");
+            }
+        }
+
+        // Initialize the ports in the BB to set the type
+        for(const auto& port_it: manifest.ports)
+        {
+            const std::string& port_name = port_it.first;
+            const auto& port_info = port_it.second;
+
+            auto remap_it = port_remap.find(port_name);
+            if( remap_it == port_remap.end())
+            {
+                continue;
+            }
+            StringView param_value = remap_it->second;
+            auto param_res = TreeNode::getRemappedKey(port_name, param_value);
+            if( param_res )
+            {
+                const auto port_key = static_cast<std::string>(param_res.value());
+
+                auto prev_info = blackboard->portInfo( port_key );
+                if( !prev_info  )
+                {
+                    // not found, insert for the first time.
+                    blackboard->setPortInfo( port_key, port_info );
+                }
+                else{
+                    // found. check consistency
+                    if( prev_info->type() && port_info.type()  && // null type means that everything is valid
+                        *prev_info->type() != *port_info.type())
+                    {
+                        blackboard->debugMessage();
+
+                        throw RuntimeError( "The creation of the tree failed because the port [", port_key,
+                                           "] was initially created with type [", demangle( prev_info->type() ),
+                                           "] and, later type [", demangle( port_info.type() ),
+                                           "] was used somewhere else." );
+                    }
+                }
+            }
+        }
+
+        // use manifest to initialize NodeConfiguration
+        for(const auto& remap_it: port_remap)
+        {
+            const auto& port_name = remap_it.first;
+            auto port_it = manifest.ports.find( port_name );
+            if( port_it != manifest.ports.end() )
+            {
+                auto direction = port_it->second.direction();
+                if( direction != PortDirection::OUTPUT )
+                {
+                    config.input_ports.insert( remap_it );
+                }
+                if( direction != PortDirection::INPUT )
+                {
+                    config.output_ports.insert( remap_it );
+                }
+            }
+        }
+        // use default value if available for empty ports. Only inputs
+        for (const auto& port_it: manifest.ports)
+        {
+            const std::string& port_name =  port_it.first;
+            const PortInfo& port_info = port_it.second;
+
+            auto direction = port_info.direction();
+            if( direction != PortDirection::OUTPUT &&
+                config.input_ports.count(port_name) == 0 &&
+                port_info.defaultValue().empty() == false)
+            {
+                config.input_ports.insert( { port_name, port_info.defaultValue() } );
+            }
+        }
+        child_node = factory.instantiateTreeNode(instance_name, ID, config);
+    }
+    else if( tree_roots.count(ID) != 0) {
+        child_node = std::make_unique<SubtreeNode>( instance_name );
+    }
+    else{
+        throw RuntimeError( ID, " is not a registered node, nor a Subtree");
+    }
+
+    if (node_parent)
+    {
+        if (auto control_parent = dynamic_cast<ControlNode*>(node_parent.get()))
+        {
+            control_parent->addChild(child_node.get());
+        }
+        if (auto decorator_parent = dynamic_cast<DecoratorNode*>(node_parent.get()))
+        {
+            decorator_parent->setChild(child_node.get());
+        }
+    }
+    return child_node;
+}
+
+void BT::XMLParser::Pimpl::recursivelyCreateTree(const std::string& tree_ID,
+                                                 Tree& output_tree,
+                                                 Blackboard::Ptr blackboard,
+                                                 const TreeNode::Ptr& root_parent)
+{
+    std::function<void(const TreeNode::Ptr&, const XMLElement*)> recursiveStep;
+
+    recursiveStep = [&](const TreeNode::Ptr& parent,
+                        const XMLElement* element)
+    {
+        auto node = createNodeFromXML(element, blackboard, parent);
+        output_tree.nodes.push_back(node);
+
+        if( node->type() == NodeType::SUBTREE )
+        {
+            if( dynamic_cast<const SubtreeNode*>(node.get()) )
+            {
+                bool is_isolated = true;
+
+                for (const XMLAttribute* attr = element->FirstAttribute(); attr != nullptr; attr = attr->Next())
+                {
+                    if( strcmp(attr->Name(), "__shared_blackboard") == 0  &&
+                        convertFromString<bool>(attr->Value()) == true )
+                    {
+                        is_isolated = false;
+                    }
+                }
+
+                if( !is_isolated )
+                {
+                    recursivelyCreateTree( node->name(), output_tree, blackboard, node );
+                }
+                else{
+                // Creating an isolated
+                auto new_bb = Blackboard::create(blackboard);
+
+                for (const XMLAttribute* attr = element->FirstAttribute(); attr != nullptr; attr = attr->Next())
+                {
+                    if( strcmp(attr->Name(), "ID") == 0 )
+                    {
+                        continue;
+                    }
+                    new_bb->addSubtreeRemapping( attr->Name(), attr->Value() );
+                }
+                output_tree.blackboard_stack.emplace_back(new_bb);
+                recursivelyCreateTree( node->name(), output_tree, new_bb, node );
+                }
+            }
+            else if( dynamic_cast<const SubtreePlusNode*>(node.get()) )
+            {
+                auto new_bb = Blackboard::create(blackboard);
+                output_tree.blackboard_stack.emplace_back(new_bb);
+                std::set<StringView> mapped_keys;
+
+                bool do_autoremap = false;
+
+                for (const XMLAttribute* attr = element->FirstAttribute(); attr != nullptr; attr = attr->Next())
+                {
+                    if( strcmp(attr->Name(), "ID") == 0 )
+                    {
+                        continue;
+                    }
+                    if( strcmp(attr->Name(), "__autoremap") == 0 )
+                    {
+                        if( convertFromString<bool>(attr->Value()) )
+                        {
+                            do_autoremap = true;
+                        }
+                        continue;
+                    }
+
+                    StringView str =  attr->Value();
+                    if( TreeNode::isBlackboardPointer(str))
+                    {
+                        StringView port_name = TreeNode::stripBlackboardPointer(str);
+                        new_bb->addSubtreeRemapping( attr->Name(), port_name);
+                        mapped_keys.insert(attr->Name());
+                    }
+                    else{
+                        new_bb->set(attr->Name(), static_cast<std::string>(str) );
+                        mapped_keys.insert(attr->Name());
+                    }
+                }
+                recursivelyCreateTree( node->name(), output_tree, new_bb, node );
+
+                if( do_autoremap )
+                {
+                    auto keys = new_bb->getKeys();
+                    for( StringView key: keys)
+                    {
+                        if( mapped_keys.count(key) == 0)
+                        {
+                            new_bb->addSubtreeRemapping( key, key );
+                        }
+                    }
+                }
+             }
+        }
+        else
+        {
+            for (auto child_element = element->FirstChildElement(); child_element;
+                 child_element = child_element->NextSiblingElement())
+            {
+                recursiveStep(node, child_element);
+            }
+        }
+    };
+
+    auto root_element = tree_roots[tree_ID]->FirstChildElement();
+
+    // start recursion
+    recursiveStep(root_parent, root_element);
+}
+
+
+std::string writeTreeNodesModelXML(const BehaviorTreeFactory& factory)
+{
+    using namespace BT_TinyXML2;
+
+    BT_TinyXML2::XMLDocument doc;
+
+    XMLElement* rootXML = doc.NewElement("root");
+    doc.InsertFirstChild(rootXML);
+
+    XMLElement* model_root = doc.NewElement("TreeNodesModel");
+    rootXML->InsertEndChild(model_root);
+
+    for (auto& model_it : factory.manifests())
+    {
+        const auto& registration_ID = model_it.first;
+        const auto& model = model_it.second;
+
+        if( factory.builtinNodes().count( registration_ID ) != 0)
+        {
+            continue;
+        }
+
+        if (model.type == NodeType::CONTROL)
+        {
+            continue;
+        }
+        XMLElement* element = doc.NewElement( toStr(model.type).c_str() );
+        element->SetAttribute("ID", model.registration_ID.c_str());
+
+        for (auto& port : model.ports)
+        {
+            const auto& port_name = port.first;
+            const auto& port_info = port.second;
+
+            XMLElement* port_element = nullptr;
+            switch(  port_info.direction() )
+            {
+            case PortDirection::INPUT:  port_element = doc.NewElement("input_port");  break;
+            case PortDirection::OUTPUT: port_element = doc.NewElement("output_port"); break;
+            case PortDirection::INOUT:  port_element = doc.NewElement("inout_port");  break;
+            }
+
+            port_element->SetAttribute("name", port_name.c_str() );
+            if( port_info.type() )
+            {
+                port_element->SetAttribute("type", BT::demangle( port_info.type() ).c_str() );
+            }
+            if( !port_info.defaultValue().empty() )
+            {
+                port_element->SetAttribute("default", port_info.defaultValue().c_str() );
+            }
+
+            if( !port_info.description().empty() )
+            {
+                port_element->SetText( port_info.description().c_str() );
+            }
+
+            element->InsertEndChild(port_element);
+        }
+
+        model_root->InsertEndChild(element);
+    }
+
+    XMLPrinter printer;
+    doc.Print(&printer);
+    return std::string(printer.CStr(), size_t(printer.CStrSize() - 1));
+}
+
+Tree buildTreeFromText(const BehaviorTreeFactory& factory, const std::string& text,
+                       const Blackboard::Ptr& blackboard)
+{
+    XMLParser parser(factory);
+    parser.loadFromText(text);
+    return parser.instantiateTree(blackboard);
+}
+
+Tree buildTreeFromFile(const BehaviorTreeFactory& factory, const std::string& filename,
+                       const Blackboard::Ptr& blackboard)
+{
+    XMLParser parser(factory);
+    parser.loadFromFile(filename);
+    return parser.instantiateTree(blackboard);
+}
+
+}
diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
new file mode 100644 (file)
index 0000000..ca86ba3
--- /dev/null
@@ -0,0 +1,58 @@
+######################################################
+# TESTS
+
+include_directories(include)
+
+set(BT_TESTS
+  src/action_test_node.cpp
+  src/condition_test_node.cpp
+  gtest_tree.cpp
+  gtest_sequence.cpp
+  gtest_parallel.cpp
+  gtest_fallback.cpp
+  gtest_factory.cpp
+  gtest_decorator.cpp
+  gtest_blackboard.cpp
+  gtest_ports.cpp
+  navigation_test.cpp
+  gtest_subtree.cpp
+  gtest_switch.cpp
+)
+
+if (BT_COROUTINES)
+    list(APPEND BT_TESTS  gtest_coroutines.cpp)
+endif()
+
+if(ament_cmake_FOUND AND BUILD_TESTING)
+
+    find_package(ament_cmake_gtest REQUIRED)
+
+    ament_add_gtest_executable(${BEHAVIOR_TREE_LIBRARY}_test ${BT_TESTS})
+    target_link_libraries(${BEHAVIOR_TREE_LIBRARY}_test ${BEHAVIOR_TREE_LIBRARY}
+                                                        bt_sample_nodes
+                                                        ${ament_LIBRARIES})
+    target_include_directories(${BEHAVIOR_TREE_LIBRARY}_test PRIVATE gtest/include)
+    include_directories($<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/3rdparty>)
+
+elseif(catkin_FOUND AND CATKIN_ENABLE_TESTING)
+
+    catkin_add_gtest(${BEHAVIOR_TREE_LIBRARY}_test ${BT_TESTS})
+    target_link_libraries(${BEHAVIOR_TREE_LIBRARY}_test ${BEHAVIOR_TREE_LIBRARY}
+                                                        bt_sample_nodes
+                                                        ${catkin_LIBRARIES})
+    target_include_directories(${BEHAVIOR_TREE_LIBRARY}_test PRIVATE gtest/include)
+
+elseif(GTEST_FOUND AND BUILD_UNIT_TESTS)
+
+    enable_testing()
+
+    add_executable(${BEHAVIOR_TREE_LIBRARY}_test ${BT_TESTS})
+    target_link_libraries(${PROJECT_NAME}_test ${BEHAVIOR_TREE_LIBRARY}
+                                                bt_sample_nodes
+                                                ${GTEST_LIBRARIES}
+                                                ${GTEST_MAIN_LIBRARIES})
+    target_include_directories(${BEHAVIOR_TREE_LIBRARY}_test PRIVATE gtest/include ${GTEST_INCLUDE_DIRS})
+
+    add_test(BehaviorTreeCoreTest ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}/${BEHAVIOR_TREE_LIBRARY}_test)
+
+endif()
diff --git a/tests/gtest_blackboard.cpp b/tests/gtest_blackboard.cpp
new file mode 100644 (file)
index 0000000..f29b49f
--- /dev/null
@@ -0,0 +1,299 @@
+/* Copyright (C) 2018-2019 Davide Faconti, Eurecat - All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#include <gtest/gtest.h>
+#include "action_test_node.h"
+#include "condition_test_node.h"
+#include "behaviortree_cpp_v3/behavior_tree.h"
+#include "behaviortree_cpp_v3/bt_factory.h"
+#include "behaviortree_cpp_v3/blackboard.h"
+#include "behaviortree_cpp_v3/xml_parsing.h"
+
+using namespace BT;
+
+class BB_TestNode: public SyncActionNode
+{
+  public:
+    BB_TestNode(const std::string& name, const NodeConfiguration& config):
+      SyncActionNode(name, config)
+    { }
+
+    NodeStatus tick()
+    {
+        int value = 0;
+        auto res = getInput<int>("in_port");
+        if(!res)
+        {
+            throw RuntimeError("BB_TestNode needs input", res.error());
+        }
+        value = res.value()*2;
+        if( !setOutput("out_port", value) )
+        {
+            throw RuntimeError("BB_TestNode failed output");
+        }
+        return NodeStatus::SUCCESS;
+    }
+
+    static PortsList providedPorts()
+    {
+        return { BT::InputPort<int>("in_port"),
+                 BT::OutputPort<int>("out_port") };
+    }
+};
+
+
+class BB_TypedTestNode: public SyncActionNode
+{
+  public:
+    BB_TypedTestNode(const std::string& name, const NodeConfiguration& config):
+      SyncActionNode(name, config)
+    { }
+
+    NodeStatus tick()
+    {
+        return NodeStatus::SUCCESS;
+    }
+
+    static PortsList providedPorts()
+    {
+        return { BT::InputPort("input"),
+                 BT::InputPort<int>("input_int"),
+                 BT::InputPort<std::string>("input_string"),
+
+                 BT::OutputPort("output"),
+                 BT::OutputPort<int>("output_int"),
+                 BT::OutputPort<std::string>("output_string") };
+    }
+};
+
+
+TEST(BlackboardTest, GetInputsFromBlackboard)
+{
+    auto bb = Blackboard::create();
+
+    NodeConfiguration config;
+    assignDefaultRemapping<BB_TestNode>( config );
+
+    config.blackboard = bb;
+    bb->set("in_port", 11 );
+
+    BB_TestNode node("good_one", config);
+
+    // this should read and write "my_entry" in tick()
+    node.executeTick();
+
+    ASSERT_EQ( bb->get<int>("out_port"), 22 );
+}
+
+TEST(BlackboardTest, BasicRemapping)
+{
+    auto bb = Blackboard::create();
+
+    NodeConfiguration config;
+
+    config.blackboard = bb;
+    config.input_ports["in_port"]   = "{my_input_port}";
+    config.output_ports["out_port"] = "{my_output_port}";
+    bb->set("my_input_port", 11 );
+
+    BB_TestNode node("good_one", config);
+    node.executeTick();
+
+    ASSERT_EQ( bb->get<int>("my_output_port"), 22 );
+}
+
+TEST(BlackboardTest, GetInputsFromText)
+{
+    auto bb = Blackboard::create();
+
+    NodeConfiguration config;
+    config.input_ports["in_port"] = "11";
+
+    BB_TestNode missing_out("missing_output", config);
+    EXPECT_THROW( missing_out.executeTick(), RuntimeError );
+
+    config.blackboard = bb;
+    config.output_ports["out_port"] = "=";
+
+    BB_TestNode node("good_one", config);
+    node.executeTick();
+
+    ASSERT_EQ( bb->get<int>("out_port"), 22 );
+}
+
+TEST(BlackboardTest, SetOutputFromText)
+{
+    const char* xml_text = R"(
+
+     <root main_tree_to_execute = "MainTree" >
+         <BehaviorTree ID="MainTree">
+            <Sequence>
+                <BB_TestNode in_port="11" out_port="{my_port}"/>
+                <SetBlackboard output_key="my_port" value="-43" />
+            </Sequence>
+         </BehaviorTree>
+     </root>
+    )";
+
+    BehaviorTreeFactory factory;
+    factory.registerNodeType<BB_TestNode>("BB_TestNode");
+
+    auto bb = Blackboard::create();
+
+    auto tree = factory.createTreeFromText(xml_text, bb);
+    tree.tickRoot();
+}
+
+TEST(BlackboardTest, WithFactory)
+{
+    BehaviorTreeFactory factory;
+
+    factory.registerNodeType<BB_TestNode>("BB_TestNode");
+
+    const std::string xml_text = R"(
+
+    <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <Sequence>
+                <BB_TestNode name = "first" in_port="11"
+                             out_port="{my_input_port}"/>
+
+                <BB_TestNode name = "second" in_port="{my_input_port}"
+                             out_port="{my_input_port}" />
+
+                <BB_TestNode name = "third" in_port="{my_input_port}"
+                             out_port="{my_output_port}" />
+            </Sequence>
+        </BehaviorTree>
+    </root>)";
+
+    auto bb = Blackboard::create();
+
+    auto tree = factory.createTreeFromText(xml_text, bb);
+    NodeStatus status = tree.tickRoot();
+
+    ASSERT_EQ( status, NodeStatus::SUCCESS );
+    ASSERT_EQ( bb->get<int>("my_input_port"), 44 );
+    ASSERT_EQ( bb->get<int>("my_output_port"), 88 );
+}
+
+
+TEST(BlackboardTest, TypoInPortName)
+{
+    BehaviorTreeFactory factory;
+    factory.registerNodeType<BB_TestNode>("BB_TestNode");
+
+    const std::string xml_text = R"(
+
+    <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+             <BB_TestNode inpuuuut_port="{value}" />
+        </BehaviorTree>
+    </root>)";
+
+    ASSERT_THROW( factory.createTreeFromText(xml_text), RuntimeError );
+}
+
+
+TEST(BlackboardTest, CheckPortType)
+{
+    BehaviorTreeFactory factory;
+    factory.registerNodeType<BB_TypedTestNode>("TypedNode");
+
+    //-----------------------------
+    std::string good_one = R"(
+    <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <Sequence>
+                <TypedNode name = "first"  output_int="{matching}"  output_string="{whatever}" output="{no_problem}" />
+                <TypedNode name = "second" input_int="{matching}"   input="{whatever}"         input_string="{no_problem}"  />
+            </Sequence>
+        </BehaviorTree>
+    </root>)";
+
+    auto tree = factory.createTreeFromText(good_one);
+    ASSERT_NE( tree.rootNode(), nullptr );
+    //-----------------------------
+    std::string bad_one = R"(
+    <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <Sequence>
+                <TypedNode name = "first"  output_int="{value}" />
+                <TypedNode name = "second" input_string="{value}" />
+            </Sequence>
+        </BehaviorTree>
+    </root>)";
+
+    ASSERT_THROW( factory.createTreeFromText(bad_one), RuntimeError);
+}
+
+class RefCountClass {
+  public:
+    RefCountClass(std::shared_ptr<int> value): sptr_(std::move(value))
+    {
+        std::cout<< "Constructor: ref_count " << sptr_.use_count() << std::endl;
+    }
+
+    RefCountClass(const RefCountClass &from) : sptr_(from.sptr_)
+    {
+        std::cout<< "ctor copy: ref_count " << sptr_.use_count() << std::endl;
+    }
+
+    RefCountClass& operator=(const RefCountClass &from)
+    {
+        sptr_ = (from.sptr_);
+        std::cout<< "equal copied: ref_count " << sptr_.use_count() << std::endl;
+        return *this;
+    }
+
+    virtual ~RefCountClass() {
+        std::cout<<("Destructor")<< std::endl;
+    }
+
+    int refCount() const { return sptr_.use_count(); }
+
+  private:
+    std::shared_ptr<int> sptr_;
+};
+
+TEST(BlackboardTest, MoveVsCopy)
+{
+    auto blackboard = Blackboard::create();
+
+    RefCountClass test( std::make_shared<int>() );
+
+    ASSERT_EQ( test.refCount(), 1);
+
+    std::cout<<("----- before set -----")<< std::endl;
+    blackboard->set("testmove", test );
+    std::cout<<(" ----- after set -----")<< std::endl;
+
+    ASSERT_EQ( test.refCount(), 2);
+
+    RefCountClass other( blackboard->get<RefCountClass>("testmove") );
+
+    ASSERT_EQ( test.refCount(), 3);
+}
+
+
+TEST(BlackboardTest, CheckTypeSafety)
+{
+    //TODO check type safety when ports are created.
+    // remember that std::string is considered a type erased type.
+
+    bool is = std::is_constructible<BT::StringView, char*>::value;
+    ASSERT_TRUE( is );
+
+    is = std::is_constructible<BT::StringView, std::string>::value;
+    ASSERT_TRUE( is );
+}
+
diff --git a/tests/gtest_coroutines.cpp b/tests/gtest_coroutines.cpp
new file mode 100644 (file)
index 0000000..d006582
--- /dev/null
@@ -0,0 +1,176 @@
+#include "behaviortree_cpp_v3/decorators/timeout_node.h"
+#include "behaviortree_cpp_v3/behavior_tree.h"
+#include <chrono>
+#include <future>
+#include <gtest/gtest.h>
+
+using namespace std::chrono;
+
+using Millisecond = std::chrono::milliseconds;
+using Timepoint = std::chrono::time_point<std::chrono::steady_clock>;
+
+class SimpleCoroAction : public BT::CoroActionNode
+{
+  public:
+    SimpleCoroAction(milliseconds timeout, bool will_fail,
+                     const std::string &node_name,
+                     const BT::NodeConfiguration &config)
+      : BT::CoroActionNode(node_name, config)
+      , will_fail_(will_fail)
+      , timeout_(timeout)
+      , start_time_(Timepoint::min())
+    {
+    }
+
+    virtual void halt() override
+    {
+        std::cout << "Action was halted. doing cleanup here" << std::endl;
+        start_time_ = Timepoint::min();
+        halted_ = true;
+        BT::CoroActionNode::halt();
+    }
+
+    bool wasHalted()
+    {
+        return halted_;
+    }
+
+    void setRequiredTime(Millisecond ms)
+    {
+        timeout_ = ms;
+    }
+
+  protected:
+    virtual BT::NodeStatus tick() override
+    {
+        std::cout << "Starting action " << std::endl;
+        halted_ = false;
+
+        if (start_time_ == Timepoint::min())
+        {
+            start_time_ = std::chrono::steady_clock::now();
+        }
+
+        while (std::chrono::steady_clock::now() < (start_time_ + timeout_))
+        {
+            setStatusRunningAndYield();
+        }
+
+        halted_ = false;
+
+        std::cout << "Done" << std::endl;
+        start_time_ = Timepoint::min();
+        return (will_fail_ ? BT::NodeStatus::FAILURE : BT::NodeStatus::SUCCESS);
+    }
+
+  public:
+    bool will_fail_;
+
+  private:
+    std::chrono::milliseconds timeout_;
+    Timepoint start_time_;
+    bool halted_;
+};
+
+BT::NodeStatus executeWhileRunning(BT::TreeNode &node)
+{
+    auto status = node.executeTick();
+    while (status == BT::NodeStatus::RUNNING)
+    {
+        status = node.executeTick();
+        std::this_thread::sleep_for(Millisecond(1));
+    }
+    return status;
+}
+
+
+TEST(CoroTest, do_action)
+{
+    BT::NodeConfiguration node_config_;
+    node_config_.blackboard = BT::Blackboard::create();
+    BT::assignDefaultRemapping<SimpleCoroAction>(node_config_);
+    SimpleCoroAction node( milliseconds(200), false, "Action", node_config_);
+
+    EXPECT_EQ(BT::NodeStatus::SUCCESS, executeWhileRunning(node));
+    EXPECT_FALSE( node.wasHalted() );
+
+    EXPECT_EQ(BT::NodeStatus::SUCCESS, executeWhileRunning(node)) << "Second call to coro action";
+    EXPECT_FALSE( node.wasHalted() );
+
+    node.will_fail_ = true;
+    EXPECT_EQ(BT::NodeStatus::FAILURE, executeWhileRunning(node))
+        << "Should execute again and retun failure";
+    EXPECT_FALSE( node.wasHalted() );
+
+
+    EXPECT_EQ(BT::NodeStatus::FAILURE, executeWhileRunning(node))
+        << "Shoudln't fail because we set status to idle";
+    EXPECT_FALSE( node.wasHalted() );
+}
+
+
+TEST(CoroTest, do_action_timeout)
+{
+    BT::NodeConfiguration node_config_;
+    node_config_.blackboard = BT::Blackboard::create();
+    BT::assignDefaultRemapping<SimpleCoroAction>(node_config_);
+
+    SimpleCoroAction node( milliseconds(300), false, "Action", node_config_);
+    BT::TimeoutNode<> timeout("TimeoutAction", 200);
+
+    timeout.setChild(&node);
+
+    EXPECT_EQ(BT::NodeStatus::FAILURE, executeWhileRunning(timeout) ) << "should timeout";
+    EXPECT_TRUE( node.wasHalted() );
+
+    node.setRequiredTime( Millisecond(100) );
+
+    EXPECT_EQ(BT::NodeStatus::SUCCESS, executeWhileRunning(timeout) );
+    EXPECT_FALSE( node.wasHalted() );
+}
+
+TEST(CoroTest, sequence_child)
+{
+    BT::NodeConfiguration node_config_;
+    node_config_.blackboard = BT::Blackboard::create();
+    BT::assignDefaultRemapping<SimpleCoroAction>(node_config_);
+
+    SimpleCoroAction actionA( milliseconds(200), false, "action_A", node_config_);
+    SimpleCoroAction actionB( milliseconds(200), false, "action_B", node_config_);
+    BT::TimeoutNode<> timeout("timeout", 300);
+    BT::SequenceNode sequence("sequence");
+
+    timeout.setChild(&sequence);
+    sequence.addChild(&actionA);
+    sequence.addChild(&actionB);
+
+    EXPECT_EQ(BT::NodeStatus::FAILURE, executeWhileRunning(timeout) ) << "should timeout";
+    EXPECT_FALSE( actionA.wasHalted() );
+    EXPECT_TRUE( actionB.wasHalted() );
+}
+
+TEST(CoroTest, OtherThreadHalt)
+{
+    BT::NodeConfiguration node_config_;
+    node_config_.blackboard = BT::Blackboard::create();
+    BT::assignDefaultRemapping<SimpleCoroAction>(node_config_);
+
+    SimpleCoroAction actionA( milliseconds(200), false, "action_A", node_config_);
+    actionA.executeTick();
+
+    std::cout << "----- 1 ------ " << std::endl;
+    auto handle = std::async(std::launch::async, [&](){
+                                 actionA.halt();
+                             });
+    handle.wait();
+    std::cout << "----- 2 ------ " << std::endl;
+    EXPECT_TRUE( actionA.wasHalted() );
+
+    std::cout << "----- 3------ " << std::endl;
+    handle = std::async(std::launch::async, [&](){
+                                 actionA.executeTick();
+                             });
+    handle.wait();
+    std::cout << "----- 4 ------ " << std::endl;
+}
+
diff --git a/tests/gtest_decorator.cpp b/tests/gtest_decorator.cpp
new file mode 100644 (file)
index 0000000..7ed19d9
--- /dev/null
@@ -0,0 +1,195 @@
+/* Copyright (C) 2018-2019 Davide Faconti, Eurecat - All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#include <gtest/gtest.h>
+#include "action_test_node.h"
+#include "condition_test_node.h"
+#include "behaviortree_cpp_v3/behavior_tree.h"
+
+using BT::NodeStatus;
+using std::chrono::milliseconds;
+
+struct DeadlineTest : testing::Test
+{
+    BT::TimeoutNode<> root;
+    BT::AsyncActionTest action;
+
+    DeadlineTest() : root("deadline", 300)
+      , action("action", milliseconds(500) )
+    {
+        root.setChild(&action);
+    }
+    ~DeadlineTest() = default;
+};
+
+struct RepeatTest : testing::Test
+{
+    BT::RepeatNode root;
+    BT::SyncActionTest action;
+
+    RepeatTest() : root("repeat", 3), action("action")
+    {
+        root.setChild(&action);
+    }
+    ~RepeatTest() = default;
+};
+
+struct RepeatTestAsync : testing::Test
+{
+    BT::RepeatNode root;
+    BT::AsyncActionTest action;
+
+    RepeatTestAsync() : root("repeat", 3), action("action", milliseconds(100))
+    {
+        root.setChild(&action);
+    }
+    ~RepeatTestAsync() = default;
+};
+
+struct RetryTest : testing::Test
+{
+    BT::RetryNode root;
+    BT::SyncActionTest action;
+
+    RetryTest() : root("retry", 3), action("action")
+    {
+        root.setChild(&action);
+    }
+    ~RetryTest() = default;
+};
+
+struct TimeoutAndRetry : testing::Test
+{
+    BT::TimeoutNode<> timeout_root;
+    BT::RetryNode retry;
+    BT::SyncActionTest action;
+
+    TimeoutAndRetry() :
+      timeout_root("deadline", 9)
+      , retry("retry", 1000)
+      , action("action")
+    {
+        timeout_root.setChild(&retry);
+        retry.setChild(&action);
+    }
+};
+
+/****************TESTS START HERE***************************/
+
+TEST_F(DeadlineTest, DeadlineTriggeredTest)
+{
+    BT::NodeStatus state = root.executeTick();
+    // deadline in 300 ms, action requires 500 ms
+
+    ASSERT_EQ(NodeStatus::RUNNING, action.status());
+    ASSERT_EQ(NodeStatus::RUNNING, state);
+
+    std::this_thread::sleep_for(std::chrono::milliseconds(400));
+    state = root.executeTick();
+    ASSERT_EQ(NodeStatus::FAILURE, state);
+    ASSERT_EQ(NodeStatus::IDLE, action.status());
+}
+
+TEST_F(DeadlineTest, DeadlineNotTriggeredTest)
+{
+    action.setTime( milliseconds(200) );
+    // deadline in 300 ms
+
+    BT::NodeStatus state = root.executeTick();
+
+    ASSERT_EQ(NodeStatus::RUNNING, action.status());
+    ASSERT_EQ(NodeStatus::RUNNING, state);
+
+    std::this_thread::sleep_for(std::chrono::milliseconds(400));
+    state = root.executeTick();
+    ASSERT_EQ(NodeStatus::IDLE, action.status());
+    ASSERT_EQ(NodeStatus::SUCCESS, state);
+}
+
+TEST_F(RetryTest, RetryTestA)
+{
+    action.setExpectedResult(NodeStatus::FAILURE);
+
+    root.executeTick();
+    ASSERT_EQ(NodeStatus::FAILURE, root.status());
+    ASSERT_EQ(3, action.tickCount() );
+
+    action.setExpectedResult(NodeStatus::SUCCESS);
+    action.resetTicks();
+
+    root.executeTick();
+    ASSERT_EQ(NodeStatus::SUCCESS, root.status());
+    ASSERT_EQ(1, action.tickCount() );
+}
+
+
+TEST_F(RepeatTestAsync, RepeatTestAsync)
+{
+    action.setExpectedResult(NodeStatus::SUCCESS);
+
+    auto res = root.executeTick();
+
+    while(res == NodeStatus::RUNNING){
+        std::this_thread::sleep_for(std::chrono::milliseconds(20));
+        res =  root.executeTick();
+    }
+
+    ASSERT_EQ(NodeStatus::SUCCESS, root.status());
+    ASSERT_EQ(3, action.successCount() );
+    ASSERT_EQ(0, action.failureCount() );
+
+    //-------------------
+    action.setExpectedResult(NodeStatus::FAILURE);
+    action.resetCounters();
+
+    res = root.executeTick();
+    while(res == NodeStatus::RUNNING){
+        std::this_thread::sleep_for(std::chrono::milliseconds(20));
+        res =  root.executeTick();
+    }
+
+    ASSERT_EQ(NodeStatus::FAILURE, root.status());
+    ASSERT_EQ(0, action.successCount() );
+    ASSERT_EQ(1, action.failureCount() );
+
+}
+
+TEST_F(RepeatTest, RepeatTestA)
+{
+    action.setExpectedResult(NodeStatus::FAILURE);
+
+    root.executeTick();
+    ASSERT_EQ(NodeStatus::FAILURE, root.status());
+    ASSERT_EQ(1, action.tickCount() );
+
+    //-------------------
+    action.resetTicks();
+    action.setExpectedResult(NodeStatus::SUCCESS);
+
+    root.executeTick();
+    ASSERT_EQ(NodeStatus::SUCCESS, root.status());
+    ASSERT_EQ(3, action.tickCount() );
+}
+
+// https://github.com/BehaviorTree/BehaviorTree.CPP/issues/57
+TEST_F(TimeoutAndRetry, Issue57)
+{
+    action.setExpectedResult(NodeStatus::FAILURE);
+
+    auto t1 = std::chrono::high_resolution_clock::now();
+
+    while( std::chrono::high_resolution_clock::now() < t1 + std::chrono::seconds(2) )
+    {
+        ASSERT_NE( timeout_root.executeTick(), BT::NodeStatus::IDLE );
+        std::this_thread::sleep_for( std::chrono::microseconds(50) );
+    }
+}
diff --git a/tests/gtest_factory.cpp b/tests/gtest_factory.cpp
new file mode 100644 (file)
index 0000000..406c5e2
--- /dev/null
@@ -0,0 +1,246 @@
+#include <gtest/gtest.h>
+#include "action_test_node.h"
+#include "condition_test_node.h"
+#include "behaviortree_cpp_v3/xml_parsing.h"
+#include "../sample_nodes/crossdoor_nodes.h"
+#include "../sample_nodes/dummy_nodes.h"
+
+using namespace BT;
+
+// clang-format off
+
+static const char* xml_text = R"(
+
+<root main_tree_to_execute = "MainTree" >
+
+    <BehaviorTree ID="MainTree">
+        <Fallback name="root_selector">
+
+            <Sequence name="door_open_sequence">
+                <Action ID="IsDoorOpen" />
+                <Action ID="PassThroughDoor" />
+            </Sequence>
+
+            <Sequence name="door_closed_sequence">
+                <Decorator ID="Inverter">
+                     <Action ID="IsDoorOpen" />
+                </Decorator>
+                <Action ID="OpenDoor" />
+                <Action ID="PassThroughDoor" />
+                <Action ID="CloseDoor" />
+            </Sequence>
+
+            <Action ID="PassThroughWindow" />
+
+        </Fallback>
+    </BehaviorTree>
+
+    <!-- TreeNodesModel is used only by the Graphic interface -->
+    <TreeNodesModel>
+        <Action ID="IsDoorOpen" />
+        <Action ID="PassThroughDoor" />
+        <Action ID="CloseDoor" />
+        <Action ID="OpenDoor" />
+        <Action ID="PassThroughWindow" />
+    </TreeNodesModel>
+</root>
+        )";
+
+static const char* xml_text_subtree = R"(
+
+<root main_tree_to_execute = "MainTree" >
+
+  <BehaviorTree ID="CrossDoorSubtree">
+    <Sequence name="door_sequence">
+      <Decorator ID="Inverter">
+        <Action ID="IsDoorLocked" />
+      </Decorator>
+      <Action ID="OpenDoor" />
+      <Action ID="PassThroughDoor" />
+      <Action ID="CloseDoor" />
+    </Sequence>
+  </BehaviorTree>
+
+  <!-- This tree will include the other one -->
+  <BehaviorTree ID="MainTree">
+    <Fallback name="root_selector">
+      <SubTree ID="CrossDoorSubtree" />
+      <Action ID="PassThroughWindow" />
+    </Fallback>
+  </BehaviorTree>
+
+</root>  )";
+
+// clang-format on
+
+TEST(BehaviorTreeFactory, VerifyLargeTree)
+{
+    BehaviorTreeFactory factory;
+    CrossDoor::RegisterNodes(factory);
+
+    Tree tree = factory.createTreeFromText(xml_text);
+
+    printTreeRecursively(tree.rootNode());
+
+    ASSERT_EQ(tree.rootNode()->name(), "root_selector");
+
+    auto fallback = dynamic_cast<const FallbackNode*>(tree.rootNode());
+    ASSERT_TRUE(fallback != nullptr);
+
+    ASSERT_EQ(fallback->children().size(), 3);
+    ASSERT_EQ(fallback->child(0)->name(), "door_open_sequence");
+    ASSERT_EQ(fallback->child(1)->name(), "door_closed_sequence");
+    ASSERT_EQ(fallback->child(2)->name(), "PassThroughWindow");
+
+    auto sequence_open = dynamic_cast<const SequenceNode*>(fallback->child(0));
+    ASSERT_TRUE(sequence_open != nullptr);
+
+    ASSERT_EQ(sequence_open->children().size(), 2);
+    ASSERT_EQ(sequence_open->child(0)->name(), "IsDoorOpen");
+    ASSERT_EQ(sequence_open->child(1)->name(), "PassThroughDoor");
+
+    auto sequence_closed = dynamic_cast<const SequenceNode*>(fallback->child(1));
+    ASSERT_TRUE(sequence_closed != nullptr);
+
+    ASSERT_EQ(sequence_closed->children().size(), 4);
+    ASSERT_EQ(sequence_closed->child(0)->name(), "Inverter");
+    ASSERT_EQ(sequence_closed->child(1)->name(), "OpenDoor");
+    ASSERT_EQ(sequence_closed->child(2)->name(), "PassThroughDoor");
+    ASSERT_EQ(sequence_closed->child(3)->name(), "CloseDoor");
+
+    auto decorator = dynamic_cast<const InverterNode*>(sequence_closed->child(0));
+    ASSERT_TRUE(decorator != nullptr);
+
+    ASSERT_EQ(decorator->child()->name(), "IsDoorOpen");
+}
+
+TEST(BehaviorTreeFactory, Subtree)
+{
+    BehaviorTreeFactory factory;
+    CrossDoor::RegisterNodes(factory);
+
+    Tree tree = factory.createTreeFromText(xml_text_subtree);
+
+    printTreeRecursively(tree.rootNode());
+
+    ASSERT_EQ(tree.rootNode()->name(), "root_selector");
+
+    auto root_selector = dynamic_cast<const FallbackNode*>(tree.rootNode());
+    ASSERT_TRUE(root_selector != nullptr);
+    ASSERT_EQ(root_selector->children().size(), 2);
+    ASSERT_EQ(root_selector->child(0)->name(), "CrossDoorSubtree");
+    ASSERT_EQ(root_selector->child(1)->name(), "PassThroughWindow");
+
+    auto subtree = dynamic_cast<const SubtreeNode*>(root_selector->child(0));
+    ASSERT_TRUE(subtree != nullptr);
+
+    auto sequence = dynamic_cast<const SequenceNode*>(subtree->child());
+    ASSERT_TRUE(sequence != nullptr);
+
+    ASSERT_EQ(sequence->children().size(), 4);
+    ASSERT_EQ(sequence->child(0)->name(), "Inverter");
+    ASSERT_EQ(sequence->child(1)->name(), "OpenDoor");
+    ASSERT_EQ(sequence->child(2)->name(), "PassThroughDoor");
+    ASSERT_EQ(sequence->child(3)->name(), "CloseDoor");
+
+    auto decorator = dynamic_cast<const InverterNode*>(sequence->child(0));
+    ASSERT_TRUE(decorator != nullptr);
+
+    ASSERT_EQ(decorator->child()->name(), "IsDoorLocked");
+}
+
+TEST(BehaviorTreeFactory, Issue7)
+{
+const std::string xml_text_issue = R"(
+<root>
+    <BehaviorTree ID="ReceiveGuest">
+    </BehaviorTree>
+</root> )";
+
+    BehaviorTreeFactory factory;
+    XMLParser parser(factory);
+
+    EXPECT_THROW( parser.loadFromText(xml_text_issue), RuntimeError );
+}
+
+
+// clang-format off
+
+static const char* xml_ports_subtree = R"(
+
+<root main_tree_to_execute = "MainTree" >
+
+  <BehaviorTree ID="TalkToMe">
+    <Sequence>
+      <SaySomething message="{hello_msg}" />
+      <SaySomething message="{bye_msg}" />
+      <SetBlackboard output_key="output" value="done!" />
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="MainTree">
+    <Sequence>
+      <SetBlackboard output_key="talk_hello" value="hello" />
+      <SetBlackboard output_key="talk_bye"   value="bye bye" />
+      <SubTree ID="TalkToMe" hello_msg="talk_hello"
+                             bye_msg="talk_bye"
+                             output="talk_out" />
+      <SaySomething message="{talk_out}" />
+    </Sequence>
+  </BehaviorTree>
+
+</root> )";
+
+// clang-format on
+
+TEST(BehaviorTreeFactory, SubTreeWithRemapping)
+{
+    BehaviorTreeFactory factory;
+    factory.registerNodeType<DummyNodes::SaySomething>("SaySomething");
+
+    Tree tree = factory.createTreeFromText(xml_ports_subtree);
+
+    auto main_bb = tree.blackboard_stack.at(0);
+    auto talk_bb = tree.blackboard_stack.at(1);
+
+    std::cout << "\n --------------------------------- \n" << std::endl;
+    main_bb->debugMessage();
+    std::cout << "\n ----- \n" << std::endl;
+    talk_bb->debugMessage();
+    std::cout << "\n --------------------------------- \n" << std::endl;
+
+    ASSERT_EQ( main_bb->portInfo("talk_hello")->type(), &typeid(std::string) );
+    ASSERT_EQ( main_bb->portInfo("talk_bye")->type(),   &typeid(std::string) );
+    ASSERT_EQ( main_bb->portInfo("talk_out")->type(),   &typeid(std::string) );
+
+    ASSERT_EQ( talk_bb->portInfo("bye_msg")->type(),   &typeid(std::string) );
+    ASSERT_EQ( talk_bb->portInfo("hello_msg")->type(), &typeid(std::string) );
+
+    // Should not throw
+    tree.tickRoot();
+
+    std::cout << "\n --------------------------------- \n" << std::endl;
+    main_bb->debugMessage();
+    std::cout << "\n ----- \n" << std::endl;
+    talk_bb->debugMessage();
+    std::cout << "\n --------------------------------- \n" << std::endl;
+
+    ASSERT_EQ( main_bb->portInfo("talk_hello")->type(), &typeid(std::string) );
+    ASSERT_EQ( main_bb->portInfo("talk_bye")->type(),   &typeid(std::string) );
+    ASSERT_EQ( main_bb->portInfo("talk_out")->type(),   &typeid(std::string) );
+
+    ASSERT_EQ( talk_bb->portInfo("bye_msg")->type(),   &typeid(std::string) );
+    ASSERT_EQ( talk_bb->portInfo("hello_msg")->type(), &typeid(std::string) );
+    ASSERT_EQ( talk_bb->portInfo("output")->type(),    &typeid(std::string) );
+
+
+    ASSERT_EQ( main_bb->get<std::string>("talk_hello"), "hello");
+    ASSERT_EQ( main_bb->get<std::string>("talk_bye"), "bye bye");
+    ASSERT_EQ( main_bb->get<std::string>("talk_out"), "done!");
+
+    // these ports should not be present in the subtree TalkToMe
+    ASSERT_FALSE( talk_bb->getAny("talk_hello") );
+    ASSERT_FALSE( talk_bb->getAny("talk_bye") );
+    ASSERT_FALSE( talk_bb->getAny("talk_out") );
+}
+
diff --git a/tests/gtest_fallback.cpp b/tests/gtest_fallback.cpp
new file mode 100644 (file)
index 0000000..df0a3a4
--- /dev/null
@@ -0,0 +1,324 @@
+/* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#include <gtest/gtest.h>
+#include "action_test_node.h"
+#include "condition_test_node.h"
+#include "behaviortree_cpp_v3/behavior_tree.h"
+
+using BT::NodeStatus;
+using std::chrono::milliseconds;
+
+struct SimpleFallbackTest : testing::Test
+{
+    BT::FallbackNode root;
+    BT::ConditionTestNode condition;
+    BT::AsyncActionTest action;
+
+    SimpleFallbackTest() :
+      root("root_fallback")
+      , condition("condition")
+      , action("action", milliseconds(100) )
+    {
+        root.addChild(&condition);
+        root.addChild(&action);
+    }
+    ~SimpleFallbackTest()
+    {
+        
+    }
+};
+
+struct ReactiveFallbackTest : testing::Test
+{
+    BT::ReactiveFallback root;
+    BT::ConditionTestNode condition_1;
+    BT::ConditionTestNode condition_2;
+    BT::AsyncActionTest action_1;
+
+    ReactiveFallbackTest()
+      : root("root_first")
+      , condition_1("condition_1")
+      , condition_2("condition_2")
+      , action_1("action_1", milliseconds(100) )
+    {
+        root.addChild(&condition_1);
+        root.addChild(&condition_2);
+        root.addChild(&action_1);
+    }
+    ~ReactiveFallbackTest()
+    {
+        
+    }
+};
+
+struct SimpleFallbackWithMemoryTest : testing::Test
+{
+    BT::FallbackNode root;
+    BT::AsyncActionTest action;
+    BT::ConditionTestNode condition;
+
+    SimpleFallbackWithMemoryTest() :
+      root("root_sequence")
+      , action("action", milliseconds(100) )
+      , condition("condition")
+    {
+        root.addChild(&condition);
+        root.addChild(&action);
+    }
+    ~SimpleFallbackWithMemoryTest()
+    {
+        
+    }
+};
+
+struct ComplexFallbackWithMemoryTest : testing::Test
+{
+    BT::FallbackNode root;
+
+    BT::AsyncActionTest action_1;
+    BT::AsyncActionTest action_2;
+
+    BT::ConditionTestNode condition_1;
+    BT::ConditionTestNode condition_2;
+
+    BT::FallbackNode fal_conditions;
+    BT::FallbackNode fal_actions;
+
+    ComplexFallbackWithMemoryTest()
+      : root("root_fallback")
+      , action_1("action_1", milliseconds(100) )
+      , action_2("action_2", milliseconds(100) )
+      , condition_1("condition_1")
+      , condition_2("condition_2")
+      , fal_conditions("fallback_conditions")
+      , fal_actions("fallback_actions")
+    {
+        root.addChild(&fal_conditions);
+        {
+            fal_conditions.addChild(&condition_1);
+            fal_conditions.addChild(&condition_2);
+        }
+        root.addChild(&fal_actions);
+        {
+            fal_actions.addChild(&action_1);
+            fal_actions.addChild(&action_2);
+        }
+    }
+    ~ComplexFallbackWithMemoryTest()
+    {
+        
+    }
+};
+
+/****************TESTS START HERE***************************/
+
+TEST_F(SimpleFallbackTest, ConditionTrue)
+{
+    // Ticking the root node
+    condition.setExpectedResult(NodeStatus::SUCCESS);
+    BT::NodeStatus state = root.executeTick();
+
+    ASSERT_EQ(NodeStatus::SUCCESS, state);
+    ASSERT_EQ(NodeStatus::IDLE, condition.status());
+    ASSERT_EQ(NodeStatus::IDLE, action.status());
+}
+
+TEST_F(SimpleFallbackTest, ConditionChangeWhileRunning)
+{
+    BT::NodeStatus state = BT::NodeStatus::IDLE;
+
+    condition.setExpectedResult(NodeStatus::FAILURE);
+    state = root.executeTick();
+
+    ASSERT_EQ(NodeStatus::RUNNING, state);
+    ASSERT_EQ(NodeStatus::FAILURE, condition.status());
+    ASSERT_EQ(NodeStatus::RUNNING, action.status());
+
+    condition.setExpectedResult(NodeStatus::SUCCESS);
+    state = root.executeTick();
+
+    ASSERT_EQ(NodeStatus::RUNNING, state);
+    ASSERT_EQ(NodeStatus::FAILURE, condition.status());
+    ASSERT_EQ(NodeStatus::RUNNING, action.status());
+}
+
+TEST_F(ReactiveFallbackTest, Condition1ToTrue)
+{
+    condition_1.setExpectedResult(NodeStatus::FAILURE);
+    condition_2.setExpectedResult(NodeStatus::FAILURE);
+
+    BT::NodeStatus state = root.executeTick();
+
+    ASSERT_EQ(NodeStatus::RUNNING, state);
+    ASSERT_EQ(NodeStatus::FAILURE, condition_1.status());
+    ASSERT_EQ(NodeStatus::FAILURE, condition_2.status());
+    ASSERT_EQ(NodeStatus::RUNNING, action_1.status());
+
+    condition_1.setExpectedResult(NodeStatus::SUCCESS);
+
+    state = root.executeTick();
+
+    ASSERT_EQ(NodeStatus::SUCCESS, state);
+    ASSERT_EQ(NodeStatus::IDLE, condition_1.status());
+    ASSERT_EQ(NodeStatus::IDLE, condition_2.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_1.status());
+}
+
+TEST_F(ReactiveFallbackTest, Condition2ToTrue)
+{
+    condition_1.setExpectedResult(NodeStatus::FAILURE);
+    condition_2.setExpectedResult(NodeStatus::FAILURE);
+
+    BT::NodeStatus state = root.executeTick();
+
+    ASSERT_EQ(NodeStatus::RUNNING, state);
+    ASSERT_EQ(NodeStatus::FAILURE, condition_1.status());
+    ASSERT_EQ(NodeStatus::FAILURE, condition_2.status());
+    ASSERT_EQ(NodeStatus::RUNNING, action_1.status());
+
+    condition_2.setExpectedResult(NodeStatus::SUCCESS);
+
+    state = root.executeTick();
+
+    ASSERT_EQ(NodeStatus::SUCCESS, state);
+    ASSERT_EQ(NodeStatus::IDLE, condition_1.status());
+    ASSERT_EQ(NodeStatus::IDLE, condition_2.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_1.status());
+}
+
+TEST_F(SimpleFallbackWithMemoryTest, ConditionFalse)
+{
+    condition.setExpectedResult(NodeStatus::FAILURE);
+    BT::NodeStatus state = root.executeTick();
+
+    ASSERT_EQ(NodeStatus::RUNNING, state);
+    ASSERT_EQ(NodeStatus::FAILURE, condition.status());
+    ASSERT_EQ(NodeStatus::RUNNING, action.status());
+}
+
+TEST_F(SimpleFallbackWithMemoryTest, ConditionTurnToTrue)
+{
+    condition.setExpectedResult(NodeStatus::FAILURE);
+    BT::NodeStatus state = root.executeTick();
+
+    ASSERT_EQ(NodeStatus::RUNNING, state);
+    ASSERT_EQ(NodeStatus::FAILURE, condition.status());
+    ASSERT_EQ(NodeStatus::RUNNING, action.status());
+
+    condition.setExpectedResult(NodeStatus::SUCCESS);
+    state = root.executeTick();
+
+    ASSERT_EQ(NodeStatus::RUNNING, state);
+    ASSERT_EQ(NodeStatus::FAILURE, condition.status());
+    ASSERT_EQ(NodeStatus::RUNNING, action.status());
+}
+
+TEST_F(ComplexFallbackWithMemoryTest, ConditionsTrue)
+{
+    BT::NodeStatus state = root.executeTick();
+
+    ASSERT_EQ(NodeStatus::SUCCESS, state);
+    ASSERT_EQ(NodeStatus::IDLE, fal_conditions.status());
+    ASSERT_EQ(NodeStatus::IDLE, condition_1.status());
+    ASSERT_EQ(NodeStatus::IDLE, condition_2.status());
+    ASSERT_EQ(NodeStatus::IDLE, fal_actions.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_1.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_2.status());
+}
+
+TEST_F(ComplexFallbackWithMemoryTest, Condition1False)
+{
+    condition_1.setExpectedResult(NodeStatus::FAILURE);
+    BT::NodeStatus state = root.executeTick();
+
+    ASSERT_EQ(NodeStatus::SUCCESS, state);
+    ASSERT_EQ(NodeStatus::IDLE, fal_conditions.status());
+    ASSERT_EQ(NodeStatus::IDLE, condition_1.status());
+    ASSERT_EQ(NodeStatus::IDLE, condition_2.status());
+    ASSERT_EQ(NodeStatus::IDLE, fal_actions.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_1.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_2.status());
+}
+
+TEST_F(ComplexFallbackWithMemoryTest, ConditionsFalse)
+{
+    condition_1.setExpectedResult(NodeStatus::FAILURE);
+    condition_2.setExpectedResult(NodeStatus::FAILURE);
+    BT::NodeStatus state = root.executeTick();
+
+    ASSERT_EQ(NodeStatus::RUNNING, state);
+    ASSERT_EQ(NodeStatus::FAILURE, fal_conditions.status());
+    ASSERT_EQ(NodeStatus::IDLE, condition_1.status());
+    ASSERT_EQ(NodeStatus::IDLE, condition_2.status());
+    ASSERT_EQ(NodeStatus::RUNNING, fal_actions.status());
+    ASSERT_EQ(NodeStatus::RUNNING, action_1.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_2.status());
+}
+
+TEST_F(ComplexFallbackWithMemoryTest, Conditions1ToTrue)
+{
+    condition_1.setExpectedResult(NodeStatus::FAILURE);
+    condition_2.setExpectedResult(NodeStatus::FAILURE);
+    BT::NodeStatus state = root.executeTick();
+
+    condition_1.setExpectedResult(NodeStatus::SUCCESS);
+    state = root.executeTick();
+
+    ASSERT_EQ(NodeStatus::RUNNING, state);
+    ASSERT_EQ(NodeStatus::FAILURE, fal_conditions.status());
+    ASSERT_EQ(NodeStatus::IDLE, condition_1.status());
+    ASSERT_EQ(NodeStatus::IDLE, condition_2.status());
+    ASSERT_EQ(NodeStatus::RUNNING, fal_actions.status());
+    ASSERT_EQ(NodeStatus::RUNNING, action_1.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_2.status());
+}
+
+TEST_F(ComplexFallbackWithMemoryTest, Conditions2ToTrue)
+{
+    condition_1.setExpectedResult(NodeStatus::FAILURE);
+    condition_2.setExpectedResult(NodeStatus::FAILURE);
+    BT::NodeStatus state = root.executeTick();
+
+    condition_2.setExpectedResult(NodeStatus::SUCCESS);
+    state = root.executeTick();
+
+    ASSERT_EQ(NodeStatus::RUNNING, state);
+    ASSERT_EQ(NodeStatus::FAILURE, fal_conditions.status());
+    ASSERT_EQ(NodeStatus::IDLE, condition_1.status());
+    ASSERT_EQ(NodeStatus::IDLE, condition_2.status());
+    ASSERT_EQ(NodeStatus::RUNNING, fal_actions.status());
+    ASSERT_EQ(NodeStatus::RUNNING, action_1.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_2.status());
+}
+
+TEST_F(ComplexFallbackWithMemoryTest, Action1Failed)
+{
+    action_1.setExpectedResult(NodeStatus::FAILURE);
+    action_2.setExpectedResult(NodeStatus::SUCCESS);
+    condition_1.setExpectedResult(NodeStatus::FAILURE);
+    condition_2.setExpectedResult(NodeStatus::FAILURE);
+
+    BT::NodeStatus state = root.executeTick();
+
+    state = root.executeTick();
+    std::this_thread::sleep_for(std::chrono::milliseconds(500));
+    state = root.executeTick();
+
+    ASSERT_EQ(NodeStatus::RUNNING, state);
+    ASSERT_EQ(NodeStatus::FAILURE, fal_conditions.status());
+    ASSERT_EQ(NodeStatus::IDLE, condition_1.status());
+    ASSERT_EQ(NodeStatus::IDLE, condition_2.status());
+    ASSERT_EQ(NodeStatus::RUNNING, fal_actions.status());
+    ASSERT_EQ(NodeStatus::FAILURE, action_1.status());
+    ASSERT_EQ(NodeStatus::RUNNING, action_2.status());
+}
diff --git a/tests/gtest_parallel.cpp b/tests/gtest_parallel.cpp
new file mode 100644 (file)
index 0000000..08f79f8
--- /dev/null
@@ -0,0 +1,318 @@
+/* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#include <gtest/gtest.h>
+#include "action_test_node.h"
+#include "condition_test_node.h"
+#include "behaviortree_cpp_v3/behavior_tree.h"
+
+using BT::NodeStatus;
+using std::chrono::milliseconds;
+
+struct SimpleParallelTest : testing::Test
+{
+    BT::ParallelNode root;
+    BT::AsyncActionTest action_1;
+    BT::ConditionTestNode condition_1;
+
+    BT::AsyncActionTest action_2;
+    BT::ConditionTestNode condition_2;
+
+    SimpleParallelTest()
+      : root("root_parallel", 4)
+      , action_1("action_1", milliseconds(100) )
+      , condition_1("condition_1")
+      , action_2("action_2", milliseconds(300))
+      , condition_2("condition_2")
+    {
+        root.addChild(&condition_1);
+        root.addChild(&action_1);
+        root.addChild(&condition_2);
+        root.addChild(&action_2);
+    }
+    ~SimpleParallelTest()
+    {
+        
+    }
+};
+
+struct ComplexParallelTest : testing::Test
+{
+    BT::ParallelNode parallel_root;
+    BT::ParallelNode parallel_left;
+    BT::ParallelNode parallel_right;
+
+    BT::AsyncActionTest action_L1;
+    BT::ConditionTestNode condition_L1;
+
+    BT::AsyncActionTest action_L2;
+    BT::ConditionTestNode condition_L2;
+
+    BT::AsyncActionTest action_R;
+    BT::ConditionTestNode condition_R;
+
+    ComplexParallelTest()
+      : parallel_root("root", 2)
+      , parallel_left("par1", 3)
+      , parallel_right("par2", 1)
+      , action_L1("action_1", milliseconds(100) )
+      , condition_L1("condition_1")
+      , action_L2("action_2", milliseconds(200) )
+      , condition_L2("condition_2")
+      , action_R("action_3", milliseconds(400) )
+      , condition_R("condition_3")
+    {
+        parallel_root.addChild(&parallel_left);
+        {
+            parallel_left.addChild(&condition_L1);
+            parallel_left.addChild(&action_L1);
+            parallel_left.addChild(&condition_L2);
+            parallel_left.addChild(&action_L2);
+        }
+        parallel_root.addChild(&parallel_right);
+        {
+            parallel_right.addChild(&condition_R);
+            parallel_right.addChild(&action_R);
+        }
+    }
+    ~ComplexParallelTest()
+    {
+    }
+};
+
+/****************TESTS START HERE***************************/
+
+TEST_F(SimpleParallelTest, ConditionsTrue)
+{
+    BT::NodeStatus state = root.executeTick();
+
+    ASSERT_EQ(NodeStatus::SUCCESS, condition_1.status());
+    ASSERT_EQ(NodeStatus::SUCCESS, condition_2.status());
+    ASSERT_EQ(NodeStatus::RUNNING, action_1.status());
+    ASSERT_EQ(NodeStatus::RUNNING, action_2.status());
+    ASSERT_EQ(NodeStatus::RUNNING, state);
+
+    std::this_thread::sleep_for( milliseconds(200) );
+    state = root.executeTick();
+
+    ASSERT_EQ(NodeStatus::SUCCESS, condition_1.status());
+    ASSERT_EQ(NodeStatus::SUCCESS, condition_2.status());
+    ASSERT_EQ(NodeStatus::SUCCESS, action_1.status());
+    ASSERT_EQ(NodeStatus::RUNNING, action_2.status());
+    ASSERT_EQ(NodeStatus::RUNNING, state);
+
+    std::this_thread::sleep_for( milliseconds(200) );
+    state = root.executeTick();
+
+    ASSERT_EQ(NodeStatus::IDLE, condition_1.status());
+    ASSERT_EQ(NodeStatus::IDLE, condition_2.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_1.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_2.status());
+    ASSERT_EQ(NodeStatus::SUCCESS, state);
+}
+
+TEST_F(SimpleParallelTest, Threshold_3)
+{
+    root.setThresholdM(3);
+    action_1.setTime( milliseconds(100) );
+    action_2.setTime( milliseconds(500) ); // this takes a lot of time
+
+    BT::NodeStatus state = root.executeTick();
+    // first tick, zero wait
+    ASSERT_EQ(NodeStatus::SUCCESS, condition_1.status());
+    ASSERT_EQ(NodeStatus::SUCCESS, condition_2.status());
+    ASSERT_EQ(NodeStatus::RUNNING, action_1.status());
+    ASSERT_EQ(NodeStatus::RUNNING, action_2.status());
+    ASSERT_EQ(NodeStatus::RUNNING, state);
+
+    std::this_thread::sleep_for( milliseconds(150) );
+    state = root.executeTick();
+    // second tick: action1 should be completed, but not action2
+    // nevertheless it is sufficient because threshold is 3
+    ASSERT_EQ(NodeStatus::IDLE, condition_1.status());
+    ASSERT_EQ(NodeStatus::IDLE, condition_2.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_1.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_2.status());
+    ASSERT_EQ(NodeStatus::SUCCESS, state);
+}
+
+TEST_F(SimpleParallelTest, Threshold_2)
+{
+    root.setThresholdM(2);
+    BT::NodeStatus state = root.executeTick();
+
+    ASSERT_EQ(NodeStatus::IDLE, condition_1.status());
+    ASSERT_EQ(NodeStatus::IDLE, condition_2.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_1.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_2.status());
+    ASSERT_EQ(NodeStatus::SUCCESS, state);
+}
+
+TEST_F(ComplexParallelTest, ConditionsTrue)
+{
+    BT::NodeStatus state = parallel_root.executeTick();
+
+    ASSERT_EQ(NodeStatus::RUNNING, parallel_left.status());
+    ASSERT_EQ(NodeStatus::SUCCESS, condition_L1.status());
+    ASSERT_EQ(NodeStatus::SUCCESS, condition_L2.status());
+    ASSERT_EQ(NodeStatus::RUNNING, action_L1.status());
+    ASSERT_EQ(NodeStatus::RUNNING, action_L2.status());
+
+    ASSERT_EQ(NodeStatus::SUCCESS, parallel_right.status());
+    ASSERT_EQ(NodeStatus::IDLE, condition_R.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_R.status());
+
+    ASSERT_EQ(NodeStatus::RUNNING, state);
+    //----------------------------------------
+    std::this_thread::sleep_for(milliseconds(200));
+    state = parallel_root.executeTick();
+
+    ASSERT_EQ(NodeStatus::IDLE, parallel_left.status());
+    ASSERT_EQ(NodeStatus::IDLE, condition_L1.status());
+    ASSERT_EQ(NodeStatus::IDLE, condition_L2.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_L1.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_L2.status());
+
+    ASSERT_EQ(NodeStatus::IDLE, parallel_right.status());
+    ASSERT_EQ(NodeStatus::IDLE, condition_R.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_R.status());
+
+    ASSERT_EQ(NodeStatus::SUCCESS, state);
+}
+
+TEST_F(ComplexParallelTest, ConditionsLeftFalse)
+{
+    parallel_left.setThresholdFM(3);
+    parallel_left.setThresholdM(3);
+    condition_L1.setExpectedResult(NodeStatus::FAILURE);
+    condition_L2.setExpectedResult(NodeStatus::FAILURE);
+    BT::NodeStatus state = parallel_root.executeTick();
+
+    // It fails because Parallel Left it will never succeed (two already fail)
+    // even though threshold_failure == 3
+
+    ASSERT_EQ(NodeStatus::IDLE, parallel_left.status());
+    ASSERT_EQ(NodeStatus::IDLE, condition_L1.status());
+    ASSERT_EQ(NodeStatus::IDLE, condition_L2.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_L1.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_L2.status());
+
+    ASSERT_EQ(NodeStatus::IDLE, parallel_right.status());
+    ASSERT_EQ(NodeStatus::IDLE, condition_R.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_R.status());
+
+    ASSERT_EQ(NodeStatus::FAILURE, state);
+}
+
+TEST_F(ComplexParallelTest, ConditionRightFalse)
+{
+    condition_R.setExpectedResult(NodeStatus::FAILURE);
+    BT::NodeStatus state = parallel_root.executeTick();
+
+    // It fails because threshold_failure is 1 for parallel right and 
+    // condition_R fails
+
+    ASSERT_EQ(NodeStatus::IDLE, parallel_left.status());
+    ASSERT_EQ(NodeStatus::IDLE, condition_L1.status());
+    ASSERT_EQ(NodeStatus::IDLE, condition_L2.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_L1.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_L2.status());
+
+    ASSERT_EQ(NodeStatus::IDLE, parallel_right.status());
+    ASSERT_EQ(NodeStatus::IDLE, condition_R.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_R.status());
+
+    ASSERT_EQ(NodeStatus::FAILURE, state);
+}
+
+TEST_F(ComplexParallelTest, ConditionRightFalse_thresholdF_2)
+{
+    parallel_right.setThresholdFM(2);
+    condition_R.setExpectedResult(NodeStatus::FAILURE);
+    BT::NodeStatus state = parallel_root.executeTick();
+
+    // All the actions are running
+
+    ASSERT_EQ(NodeStatus::RUNNING, parallel_left.status());
+    ASSERT_EQ(NodeStatus::SUCCESS, condition_L1.status());
+    ASSERT_EQ(NodeStatus::SUCCESS, condition_L2.status());
+    ASSERT_EQ(NodeStatus::RUNNING, action_L1.status());
+    ASSERT_EQ(NodeStatus::RUNNING, action_L2.status());
+
+    ASSERT_EQ(NodeStatus::RUNNING, parallel_right.status());
+    ASSERT_EQ(NodeStatus::FAILURE, condition_R.status());
+    ASSERT_EQ(NodeStatus::RUNNING, action_R.status());
+
+    ASSERT_EQ(NodeStatus::RUNNING, state);
+
+    //----------------------------------------
+    std::this_thread::sleep_for(milliseconds(500));
+    state = parallel_root.executeTick();
+
+    ASSERT_EQ(NodeStatus::IDLE, parallel_left.status());
+    ASSERT_EQ(NodeStatus::IDLE, condition_L1.status());
+    ASSERT_EQ(NodeStatus::IDLE, condition_L2.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_L1.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_L2.status());
+
+    ASSERT_EQ(NodeStatus::IDLE, parallel_right.status());
+    ASSERT_EQ(NodeStatus::IDLE, condition_R.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_R.status());
+
+    ASSERT_EQ(NodeStatus::SUCCESS, state);
+}
+
+TEST_F(ComplexParallelTest, ConditionRightFalseAction1Done)
+{
+    condition_R.setExpectedResult(NodeStatus::FAILURE);
+
+    parallel_right.setThresholdFM(2);
+    parallel_left.setThresholdM(4);
+
+    BT::NodeStatus state = parallel_root.executeTick();
+    std::this_thread::sleep_for(milliseconds(300));
+
+    // parallel_1 hasn't realize (yet) that action_1 has succeeded
+    ASSERT_EQ(NodeStatus::RUNNING, parallel_left.status());
+    ASSERT_EQ(NodeStatus::SUCCESS, condition_L1.status());
+    ASSERT_EQ(NodeStatus::SUCCESS, condition_L2.status());
+    ASSERT_EQ(NodeStatus::SUCCESS, action_L1.status());
+    ASSERT_EQ(NodeStatus::SUCCESS, action_L2.status());
+
+    ASSERT_EQ(NodeStatus::RUNNING, parallel_right.status());
+
+    //------------------------
+    state = parallel_root.executeTick();
+
+    ASSERT_EQ(NodeStatus::SUCCESS, parallel_left.status());
+    ASSERT_EQ(NodeStatus::IDLE, condition_L1.status());
+    ASSERT_EQ(NodeStatus::IDLE, condition_L2.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_L1.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_L2.status());
+
+    ASSERT_EQ(NodeStatus::RUNNING, parallel_right.status());
+    ASSERT_EQ(NodeStatus::RUNNING, action_R.status());
+
+    ASSERT_EQ(NodeStatus::RUNNING, state);
+
+    //----------------------------------
+    std::this_thread::sleep_for(milliseconds(300));
+    state = parallel_root.executeTick();
+
+    ASSERT_EQ(NodeStatus::IDLE, parallel_left.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_L1.status());
+
+    ASSERT_EQ(NodeStatus::IDLE, parallel_right.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_R.status());
+
+    ASSERT_EQ(NodeStatus::SUCCESS, state);
+}
diff --git a/tests/gtest_ports.cpp b/tests/gtest_ports.cpp
new file mode 100644 (file)
index 0000000..1e654cd
--- /dev/null
@@ -0,0 +1,53 @@
+#include <gtest/gtest.h>
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+using namespace BT;
+
+class NodeWithPorts: public SyncActionNode
+{
+  public:
+    NodeWithPorts(const std::string & name, const NodeConfiguration & config)
+    : SyncActionNode(name, config)
+    {
+        std::cout << "ctor" << std::endl;
+    }
+
+      NodeStatus tick()
+    {
+        int val_A = 0;
+        int val_B = 0;
+        if( getInput("in_port_A", val_A) &&
+            getInput("in_port_B", val_B) &&
+            val_A == 42 && val_B == 66)
+        {
+            return NodeStatus::SUCCESS;
+        }
+        return NodeStatus::FAILURE;
+    }
+
+    static PortsList providedPorts()
+    {
+        return { BT::InputPort<int>("in_port_A", 42, "magic_number"),
+                 BT::InputPort<int>("in_port_B") };
+    }
+};
+
+TEST(PortTest, DefaultPorts)
+{
+    std::string xml_txt = R"(
+    <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <NodeWithPorts name = "first"  in_port_B="66" />
+        </BehaviorTree>
+    </root>)";
+
+    BehaviorTreeFactory factory;
+    factory.registerNodeType<NodeWithPorts>("NodeWithPorts");
+
+    auto tree = factory.createTreeFromText(xml_txt);
+
+    NodeStatus status = tree.tickRoot();
+    ASSERT_EQ( status, NodeStatus::SUCCESS );
+
+}
+
diff --git a/tests/gtest_sequence.cpp b/tests/gtest_sequence.cpp
new file mode 100644 (file)
index 0000000..1141a41
--- /dev/null
@@ -0,0 +1,460 @@
+/* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#include <gtest/gtest.h>
+#include "action_test_node.h"
+#include "condition_test_node.h"
+#include "behaviortree_cpp_v3/behavior_tree.h"
+
+using BT::NodeStatus;
+using std::chrono::milliseconds;
+
+struct SimpleSequenceTest : testing::Test
+{
+    BT::SequenceNode root;
+    BT::AsyncActionTest action;
+    BT::ConditionTestNode condition;
+
+    SimpleSequenceTest() :
+      root("root_sequence")
+      , action("action", milliseconds(100))
+      , condition("condition")
+    {
+        root.addChild(&condition);
+        root.addChild(&action);
+    }
+    ~SimpleSequenceTest()
+    {
+
+    }
+};
+
+struct ComplexSequenceTest : testing::Test
+{
+    BT::ReactiveSequence root;
+    BT::AsyncActionTest action_1;
+    BT::ConditionTestNode condition_1;
+    BT::ConditionTestNode condition_2;
+
+    BT::SequenceNode seq_conditions;
+
+    ComplexSequenceTest()
+      : root("root")
+      , action_1("action_1", milliseconds(100))
+      , condition_1("condition_1")
+      , condition_2("condition_2")
+      , seq_conditions("sequence_conditions")
+    {
+        root.addChild(&seq_conditions);
+        {
+            seq_conditions.addChild(&condition_1);
+            seq_conditions.addChild(&condition_2);
+        }
+        root.addChild(&action_1);
+    }
+    ~ComplexSequenceTest()
+    {
+
+    }
+};
+
+struct SequenceTripleActionTest : testing::Test
+{
+    BT::SequenceNode root;
+    BT::ConditionTestNode condition;
+    BT::AsyncActionTest action_1;
+    BT::SyncActionTest action_2;
+    BT::AsyncActionTest action_3;
+
+    SequenceTripleActionTest()
+      : root("root_sequence")
+      , condition("condition")
+      , action_1("action_1", milliseconds(100))
+      , action_2("action_2")
+      , action_3("action_3", milliseconds(100))
+    {
+        root.addChild(&condition);
+        root.addChild(&action_1);
+        root.addChild(&action_2);
+        root.addChild(&action_3);
+    }
+    ~SequenceTripleActionTest()
+    {
+
+    }
+};
+
+struct ComplexSequence2ActionsTest : testing::Test
+{
+    BT::SequenceNode root;
+    BT::AsyncActionTest action_1;
+    BT::AsyncActionTest action_2;
+    BT::SequenceNode seq_1;
+    BT::SequenceNode seq_2;
+
+    BT::ConditionTestNode condition_1;
+    BT::ConditionTestNode condition_2;
+
+    ComplexSequence2ActionsTest()
+      : root("root_sequence")
+      , action_1("action_1", milliseconds(100))
+      , action_2("action_2", milliseconds(100))
+      , seq_1("sequence_1")
+      , seq_2("sequence_2")
+      , condition_1("condition_1")
+      , condition_2("condition_2")
+    {
+        root.addChild(&seq_1);
+        {
+            seq_1.addChild(&condition_1);
+            seq_1.addChild(&action_1);
+        }
+        root.addChild(&seq_2);
+        {
+            seq_2.addChild(&condition_2);
+            seq_2.addChild(&action_2);
+        }
+    }
+    ~ComplexSequence2ActionsTest()
+    {
+
+    }
+};
+
+struct SimpleSequenceWithMemoryTest : testing::Test
+{
+    BT::SequenceStarNode root;
+    BT::AsyncActionTest action;
+    BT::ConditionTestNode condition;
+
+    SimpleSequenceWithMemoryTest() :
+      root("root_sequence")
+      , action("action", milliseconds(100))
+      , condition("condition")
+    {
+        root.addChild(&condition);
+        root.addChild(&action);
+    }
+    ~SimpleSequenceWithMemoryTest()
+    {
+
+    }
+};
+
+struct ComplexSequenceWithMemoryTest : testing::Test
+{
+    BT::SequenceStarNode root;
+
+    BT::AsyncActionTest action_1;
+    BT::AsyncActionTest action_2;
+
+    BT::ConditionTestNode condition_1;
+    BT::ConditionTestNode condition_2;
+
+    BT::SequenceStarNode seq_conditions;
+    BT::SequenceStarNode seq_actions;
+
+    ComplexSequenceWithMemoryTest()
+      : root("root_sequence")
+      , action_1("action_1", milliseconds(100))
+      , action_2("action_2", milliseconds(100))
+      , condition_1("condition_1")
+      , condition_2("condition_2")
+      , seq_conditions("sequence_conditions")
+      , seq_actions("sequence_actions")
+    {
+        root.addChild(&seq_conditions);
+        {
+            seq_conditions.addChild(&condition_1);
+            seq_conditions.addChild(&condition_2);
+        }
+        root.addChild(&seq_actions);
+        {
+            seq_actions.addChild(&action_1);
+            seq_actions.addChild(&action_2);
+        }
+    }
+    ~ComplexSequenceWithMemoryTest()
+    {
+
+    }
+};
+
+struct SimpleParallelTest : testing::Test
+{
+    BT::ParallelNode root;
+    BT::AsyncActionTest action_1;
+    BT::ConditionTestNode condition_1;
+
+    BT::AsyncActionTest action_2;
+    BT::ConditionTestNode condition_2;
+
+    SimpleParallelTest()
+      : root("root_parallel", 4)
+      , action_1("action_1", milliseconds(100))
+      , condition_1("condition_1")
+      , action_2("action_2", milliseconds(100))
+      , condition_2("condition_2")
+    {
+        root.addChild(&condition_1);
+        root.addChild(&action_1);
+        root.addChild(&condition_2);
+        root.addChild(&action_2);
+    }
+    ~SimpleParallelTest()
+    {
+
+    }
+};
+
+/****************TESTS START HERE***************************/
+
+TEST_F(SimpleSequenceTest, ConditionTrue)
+{
+    std::cout << "Ticking the root node !" << std::endl << std::endl;
+    // Ticking the root node
+    BT::NodeStatus state = root.executeTick();
+
+    ASSERT_EQ(NodeStatus::RUNNING, action.status());
+    ASSERT_EQ(NodeStatus::RUNNING, state);
+}
+
+TEST_F(SimpleSequenceTest, ConditionTurnToFalse)
+{
+    condition.setExpectedResult(NodeStatus::FAILURE);
+    BT::NodeStatus state = root.executeTick();
+
+    state = root.executeTick();
+    ASSERT_EQ(NodeStatus::FAILURE, state);
+    ASSERT_EQ(NodeStatus::IDLE, condition.status());
+    ASSERT_EQ(NodeStatus::IDLE, action.status());
+}
+
+TEST_F(ComplexSequenceTest, ComplexSequenceConditionsTrue)
+{
+    BT::NodeStatus state = root.executeTick();
+
+    ASSERT_EQ(NodeStatus::RUNNING, state);
+    ASSERT_EQ(NodeStatus::SUCCESS, seq_conditions.status());
+    ASSERT_EQ(NodeStatus::IDLE, condition_1.status());
+    ASSERT_EQ(NodeStatus::IDLE, condition_1.status());
+    ASSERT_EQ(NodeStatus::RUNNING, action_1.status());
+}
+
+TEST_F(SequenceTripleActionTest, TripleAction)
+{
+    using namespace BT;
+    using namespace std::chrono;
+    const auto timeout = system_clock::now() + milliseconds(650);
+
+    action_1.setTime( milliseconds(300) );
+    action_3.setTime( milliseconds(300) );
+    // the sequence is supposed to finish in (300 ms * 2) = 600 ms
+
+    // first tick
+    NodeStatus state = root.executeTick();
+
+    ASSERT_EQ(NodeStatus::RUNNING, state);
+    ASSERT_EQ(NodeStatus::RUNNING, action_1.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_2.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_3.status());
+
+    // continue until succesful
+    while (state != NodeStatus::SUCCESS && system_clock::now() < timeout)
+    {
+        std::this_thread::sleep_for(milliseconds(10));
+        state = root.executeTick();
+    }
+
+    ASSERT_EQ(NodeStatus::SUCCESS, state);
+
+    // Condition is called only once
+    ASSERT_EQ(condition.tickCount(), 1);
+    // all the actions are called only once
+    ASSERT_EQ(action_1.tickCount(), 1);
+    ASSERT_EQ(action_2.tickCount(), 1);
+    ASSERT_EQ(action_3.tickCount(), 1);
+
+    ASSERT_EQ(NodeStatus::IDLE, action_1.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_2.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_3.status());
+    ASSERT_TRUE(system_clock::now() < timeout);   // no timeout should occur
+}
+
+TEST_F(ComplexSequence2ActionsTest, ConditionsTrue)
+{
+    BT::NodeStatus state = root.executeTick();
+
+    state = root.executeTick();
+
+    ASSERT_EQ(NodeStatus::RUNNING, state);
+    ASSERT_EQ(NodeStatus::RUNNING, seq_1.status());
+    ASSERT_EQ(NodeStatus::SUCCESS, condition_1.status());
+    ASSERT_EQ(NodeStatus::RUNNING, action_1.status());
+    ASSERT_EQ(NodeStatus::IDLE, seq_2.status());
+    ASSERT_EQ(NodeStatus::IDLE, condition_2.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_2.status());
+
+    std::this_thread::sleep_for(milliseconds(300));
+    state = root.executeTick();
+
+    ASSERT_EQ(NodeStatus::RUNNING, state);
+    ASSERT_EQ(NodeStatus::SUCCESS, seq_1.status());
+    ASSERT_EQ(NodeStatus::IDLE, condition_1.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_1.status());
+    ASSERT_EQ(NodeStatus::RUNNING, seq_2.status());
+    ASSERT_EQ(NodeStatus::SUCCESS, condition_2.status());
+    ASSERT_EQ(NodeStatus::RUNNING, action_2.status());
+
+    state = root.executeTick();
+}
+
+TEST_F(ComplexSequenceTest, ComplexSequenceConditions1ToFalse)
+{
+    BT::NodeStatus state = root.executeTick();
+
+    condition_1.setExpectedResult(NodeStatus::FAILURE);
+
+    state = root.executeTick();
+
+    ASSERT_EQ(NodeStatus::FAILURE, state);
+    ASSERT_EQ(NodeStatus::IDLE, seq_conditions.status());
+    ASSERT_EQ(NodeStatus::IDLE, condition_1.status());
+    ASSERT_EQ(NodeStatus::IDLE, condition_2.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_1.status());
+}
+
+TEST_F(ComplexSequenceTest, ComplexSequenceConditions2ToFalse)
+{
+    BT::NodeStatus state = root.executeTick();
+
+    condition_2.setExpectedResult(NodeStatus::FAILURE);
+
+    state = root.executeTick();
+
+    ASSERT_EQ(NodeStatus::FAILURE, state);
+    ASSERT_EQ(NodeStatus::IDLE, seq_conditions.status());
+    ASSERT_EQ(NodeStatus::IDLE, condition_1.status());
+    ASSERT_EQ(NodeStatus::IDLE, condition_2.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_1.status());
+}
+
+TEST_F(SimpleSequenceWithMemoryTest, ConditionTrue)
+{
+    BT::NodeStatus state = root.executeTick();
+    std::this_thread::sleep_for( milliseconds(50) );
+
+    ASSERT_EQ(NodeStatus::RUNNING, state);
+    ASSERT_EQ(NodeStatus::SUCCESS, condition.status());
+    ASSERT_EQ(NodeStatus::RUNNING, action.status());
+}
+
+TEST_F(SimpleSequenceWithMemoryTest, ConditionTurnToFalse)
+{
+    BT::NodeStatus state = root.executeTick();
+
+    ASSERT_EQ(NodeStatus::RUNNING, state);
+    ASSERT_EQ(NodeStatus::SUCCESS, condition.status());
+    ASSERT_EQ(NodeStatus::RUNNING, action.status());
+
+    condition.setExpectedResult(NodeStatus::FAILURE);
+    state = root.executeTick();
+
+    ASSERT_EQ(NodeStatus::RUNNING, state);
+    ASSERT_EQ(NodeStatus::SUCCESS, condition.status());
+    ASSERT_EQ(NodeStatus::RUNNING, action.status());
+}
+
+TEST_F(ComplexSequenceWithMemoryTest, ConditionsTrue)
+{
+    BT::NodeStatus state = root.executeTick();
+
+    ASSERT_EQ(NodeStatus::RUNNING, state);
+    ASSERT_EQ(NodeStatus::SUCCESS, seq_conditions.status());
+    ASSERT_EQ(NodeStatus::IDLE, condition_1.status());
+    ASSERT_EQ(NodeStatus::IDLE, condition_2.status());
+    ASSERT_EQ(NodeStatus::RUNNING, seq_actions.status());
+    ASSERT_EQ(NodeStatus::RUNNING, action_1.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_2.status());
+}
+
+TEST_F(ComplexSequenceWithMemoryTest, Conditions1ToFase)
+{
+    BT::NodeStatus state = root.executeTick();
+
+    condition_1.setExpectedResult(NodeStatus::FAILURE);
+    state = root.executeTick();
+    // change in condition_1 does not affect the state of the tree,
+    // since the seq_conditions was executed already
+    ASSERT_EQ(NodeStatus::RUNNING, state);
+    ASSERT_EQ(NodeStatus::SUCCESS, seq_conditions.status());
+    ASSERT_EQ(NodeStatus::IDLE, condition_1.status());
+    ASSERT_EQ(NodeStatus::IDLE, condition_2.status());
+    ASSERT_EQ(NodeStatus::RUNNING, seq_actions.status());
+    ASSERT_EQ(NodeStatus::RUNNING, action_1.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_2.status());
+}
+
+TEST_F(ComplexSequenceWithMemoryTest, Conditions2ToFalse)
+{
+    BT::NodeStatus state = root.executeTick();
+
+    condition_2.setExpectedResult(NodeStatus::FAILURE);
+    state = root.executeTick();
+    // change in condition_2 does not affect the state of the tree,
+    // since the seq_conditions was executed already
+    ASSERT_EQ(NodeStatus::RUNNING, state);
+    ASSERT_EQ(NodeStatus::SUCCESS, seq_conditions.status());
+    ASSERT_EQ(NodeStatus::IDLE, condition_1.status());
+    ASSERT_EQ(NodeStatus::IDLE, condition_2.status());
+    ASSERT_EQ(NodeStatus::RUNNING, seq_actions.status());
+    ASSERT_EQ(NodeStatus::RUNNING, action_1.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_2.status());
+}
+
+TEST_F(ComplexSequenceWithMemoryTest, Action1DoneSeq)
+{
+    root.executeTick();
+
+    condition_2.setExpectedResult(NodeStatus::FAILURE);
+    root.executeTick();
+
+    // change in condition_2 does not affect the state of the tree,
+    // since the seq_conditions was executed already
+    ASSERT_EQ(NodeStatus::SUCCESS, seq_conditions.status());
+    ASSERT_EQ(NodeStatus::IDLE, condition_1.status());
+    ASSERT_EQ(NodeStatus::IDLE, condition_2.status());
+    ASSERT_EQ(NodeStatus::RUNNING, seq_actions.status());
+    ASSERT_EQ(NodeStatus::RUNNING, action_1.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_2.status());
+
+    std::this_thread::sleep_for(milliseconds(150));
+    root.executeTick();
+
+    ASSERT_EQ(NodeStatus::SUCCESS, seq_conditions.status());
+    ASSERT_EQ(NodeStatus::IDLE, condition_1.status());
+    ASSERT_EQ(NodeStatus::IDLE, condition_2.status());
+    ASSERT_EQ(NodeStatus::RUNNING, seq_actions.status());
+    ASSERT_EQ(NodeStatus::SUCCESS, action_1.status());
+    ASSERT_EQ(NodeStatus::RUNNING, action_2.status());
+
+    std::this_thread::sleep_for(milliseconds(150));
+    root.executeTick();
+
+    ASSERT_EQ(NodeStatus::SUCCESS, root.status());
+    ASSERT_EQ(NodeStatus::IDLE, seq_conditions.status());
+    ASSERT_EQ(NodeStatus::IDLE, condition_1.status());
+    ASSERT_EQ(NodeStatus::IDLE, condition_2.status());
+    ASSERT_EQ(NodeStatus::IDLE, seq_actions.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_1.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_2.status());
+}
diff --git a/tests/gtest_subtree.cpp b/tests/gtest_subtree.cpp
new file mode 100644 (file)
index 0000000..10ee486
--- /dev/null
@@ -0,0 +1,236 @@
+#include <gtest/gtest.h>
+#include "behaviortree_cpp_v3/bt_factory.h"
+#include "../sample_nodes/dummy_nodes.h"
+
+using namespace BT;
+
+TEST(SubTree, SiblingPorts_Issue_72)
+{
+
+static const char* xml_text = R"(
+
+<root main_tree_to_execute = "MainTree" >
+
+    <BehaviorTree ID="MainTree">
+        <Sequence>
+            <SetBlackboard value="hello" output_key="myParam" />
+            <SubTree ID="mySubtree" param="myParam" />
+            <SetBlackboard value="world" output_key="myParam" />
+            <SubTree ID="mySubtree" param="myParam" />
+        </Sequence>
+    </BehaviorTree>
+
+    <BehaviorTree ID="mySubtree">
+            <SaySomething ID="AlwaysSuccess" message="{param}" />
+    </BehaviorTree>
+</root> )";
+
+    BehaviorTreeFactory factory;
+    factory.registerNodeType<DummyNodes::SaySomething>("SaySomething");
+
+    Tree tree = factory.createTreeFromText(xml_text);
+
+    for( auto& bb: tree.blackboard_stack)
+    {
+        bb->debugMessage();
+        std::cout << "-----" << std::endl;
+    }
+
+    auto ret = tree.tickRoot();
+
+    ASSERT_EQ(ret, NodeStatus::SUCCESS );
+    ASSERT_EQ(tree.blackboard_stack.size(), 3 );
+}
+
+class CopyPorts : public BT::SyncActionNode
+{
+public:
+  CopyPorts(const std::string& name, const BT::NodeConfiguration& config)
+  : BT::SyncActionNode(name, config)
+  {
+  }
+
+  BT::NodeStatus tick() override
+  {
+    auto msg = getInput<std::string>("in");
+    if (!msg)
+    {
+      throw BT::RuntimeError( "missing required input [message]: ", msg.error() );
+    }
+    setOutput("out", msg.value());
+    return BT::NodeStatus::SUCCESS;
+  }
+
+  static BT::PortsList providedPorts()
+  {
+    return{ BT::InputPort<std::string>("in"),
+            BT::OutputPort<std::string>("out")};
+  }
+};
+
+
+TEST(SubTree, GoodRemapping)
+{
+  static const char* xml_text = R"(
+
+<root main_tree_to_execute = "MainTree" >
+
+    <BehaviorTree ID="MainTree">
+        <Sequence>
+            <SetBlackboard value="hello" output_key="thoughts" />
+            <SubTree ID="CopySubtree" in_arg="thoughts" out_arg="greetings"/>
+            <SaySomething  message="{greetings}" />
+        </Sequence>
+    </BehaviorTree>
+
+    <BehaviorTree ID="CopySubtree">
+            <CopyPorts in="{in_arg}" out="{out_arg}"/>
+    </BehaviorTree>
+</root> )";
+
+  BehaviorTreeFactory factory;
+  factory.registerNodeType<DummyNodes::SaySomething>("SaySomething");
+  factory.registerNodeType<CopyPorts>("CopyPorts");
+
+  Tree tree = factory.createTreeFromText(xml_text);
+  auto ret = tree.tickRoot();
+  ASSERT_EQ(ret, NodeStatus::SUCCESS );
+}
+
+TEST(SubTree, BadRemapping)
+{
+  BehaviorTreeFactory factory;
+  factory.registerNodeType<DummyNodes::SaySomething>("SaySomething");
+  factory.registerNodeType<CopyPorts>("CopyPorts");
+
+  static const char* xml_text_bad_in = R"(
+<root main_tree_to_execute = "MainTree" >
+
+    <BehaviorTree ID="MainTree">
+        <Sequence>
+            <SetBlackboard value="hello" output_key="thoughts" />
+            <SubTree ID="CopySubtree" out_arg="greetings"/>
+            <SaySomething  message="{greetings}" />
+        </Sequence>
+    </BehaviorTree>
+
+    <BehaviorTree ID="CopySubtree">
+            <CopyPorts in="{in_arg}" out="{out_arg}"/>
+    </BehaviorTree>
+</root> )";
+
+  Tree tree_bad_in = factory.createTreeFromText(xml_text_bad_in);
+  EXPECT_ANY_THROW( tree_bad_in.tickRoot() );
+
+  static const char* xml_text_bad_out = R"(
+<root main_tree_to_execute = "MainTree" >
+
+    <BehaviorTree ID="MainTree">
+        <Sequence>
+            <SetBlackboard value="hello" output_key="thoughts" />
+            <SubTree ID="CopySubtree" in_arg="thoughts"/>
+            <SaySomething  message="{greetings}" />
+        </Sequence>
+    </BehaviorTree>
+
+    <BehaviorTree ID="CopySubtree">
+            <CopyPorts in="{in_arg}" out="{out_arg}"/>
+    </BehaviorTree>
+</root> )";
+
+  Tree tree_bad_out = factory.createTreeFromText(xml_text_bad_out);
+  EXPECT_ANY_THROW( tree_bad_out.tickRoot() );
+}
+
+TEST(SubTree, SubtreePlusA)
+{
+    static const char* xml_text = R"(
+
+<root main_tree_to_execute = "MainTree" >
+
+    <BehaviorTree ID="MainTree">
+        <Sequence>
+            <SetBlackboard value="Hello" output_key="myParam" />
+            <SubTreePlus ID="mySubtree" param="{myParam}" />
+            <SubTreePlus ID="mySubtree" param="World" />
+            <SetBlackboard value="Auto remapped" output_key="param" />
+            <SubTreePlus ID="mySubtree" __autoremap="1"  />
+        </Sequence>
+    </BehaviorTree>
+
+    <BehaviorTree ID="mySubtree">
+            <SaySomething message="{param}" />
+    </BehaviorTree>
+</root> )";
+
+    BehaviorTreeFactory factory;
+    factory.registerNodeType<DummyNodes::SaySomething>("SaySomething");
+
+    Tree tree = factory.createTreeFromText(xml_text);
+    auto ret = tree.tickRoot();
+    ASSERT_EQ(ret, NodeStatus::SUCCESS );
+}
+
+TEST(SubTree, SubtreePlusB)
+{
+  static const char* xml_text = R"(
+
+<root main_tree_to_execute = "MainTree" >
+
+    <BehaviorTree ID="MainTree">
+        <Sequence>
+            <SetBlackboard value="Hello World" output_key="myParam" />
+            <SetBlackboard value="Auto remapped" output_key="param3" />
+            <SubTreePlus ID="mySubtree" __autoremap="1" param1="{myParam}" param2="Straight Talking" />
+        </Sequence>
+    </BehaviorTree>
+
+    <BehaviorTree ID="mySubtree">
+        <Sequence>
+            <SaySomething message="{param1}" />
+            <SaySomething message="{param2}" />
+            <SaySomething message="{param3}" />
+        </Sequence>
+    </BehaviorTree>
+</root> )";
+
+  BehaviorTreeFactory factory;
+  factory.registerNodeType<DummyNodes::SaySomething>("SaySomething");
+
+  Tree tree = factory.createTreeFromText(xml_text);
+  auto ret = tree.tickRoot();
+  ASSERT_EQ(ret, NodeStatus::SUCCESS );
+}
+
+TEST(SubTree, SubtreePlusC)
+{
+    static const char* xml_text = R"(
+
+<root main_tree_to_execute = "MainTree" >
+
+    <BehaviorTree ID="MainTree">
+        <Sequence>
+            <SetBlackboard value="Hello" output_key="param1" />
+            <SetBlackboard value="World" output_key="param2" />
+            <SubTree ID="mySubtree" __shared_blackboard="true"/>
+        </Sequence>
+    </BehaviorTree>
+
+    <BehaviorTree ID="mySubtree">
+        <Sequence>
+            <SaySomething message="{param1}" />
+            <SaySomething message="{param2}" />
+        </Sequence>
+    </BehaviorTree>
+</root> )";
+
+    BehaviorTreeFactory factory;
+    factory.registerNodeType<DummyNodes::SaySomething>("SaySomething");
+
+    Tree tree = factory.createTreeFromText(xml_text);
+    auto ret = tree.tickRoot();
+    ASSERT_EQ(ret, NodeStatus::SUCCESS );
+}
+
+
+
diff --git a/tests/gtest_switch.cpp b/tests/gtest_switch.cpp
new file mode 100644 (file)
index 0000000..06c867c
--- /dev/null
@@ -0,0 +1,226 @@
+#include <gtest/gtest.h>
+#include "action_test_node.h"
+#include "condition_test_node.h"
+#include "behaviortree_cpp_v3/behavior_tree.h"
+#include "behaviortree_cpp_v3/tree_node.h"
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+using BT::NodeStatus;
+using std::chrono::milliseconds;
+
+static const char* xml_text = R"(
+
+<root main_tree_to_execute = "MainTree" >
+
+    <BehaviorTree ID="MainTree">
+        <Switch3 name="simple_switch" variable="{my_var}"  case_1="1" case_2="42 case_3="666" >
+            <AsyncActionTest name="action_1"/>
+            <AsyncActionTest name="action_42"/>
+            <AsyncActionTest name="action_666"/>
+            <AsyncActionTest name="action_default"/>
+        </Switch3>
+    </BehaviorTree>
+</root>
+        )";
+
+struct SwitchTest : testing::Test
+{
+    using Switch2 = BT::SwitchNode<2>;
+    std::unique_ptr<Switch2> root;
+    BT::AsyncActionTest action_1;
+    BT::AsyncActionTest action_42;
+    BT::AsyncActionTest action_def;
+    BT::Blackboard::Ptr bb = BT::Blackboard::create();
+    BT::NodeConfiguration simple_switch_config_;
+
+    SwitchTest() :
+      action_1("action_1", milliseconds(100)),
+      action_42("action_42", milliseconds(100)),
+      action_def("action_default", milliseconds(100))
+    {
+        BT::PortsRemapping input;
+        input.insert(std::make_pair("variable", "{my_var}"));
+        input.insert(std::make_pair("case_1", "1"));
+        input.insert(std::make_pair("case_2", "42"));
+
+        BT::NodeConfiguration simple_switch_config_;
+        simple_switch_config_.blackboard = bb;
+        simple_switch_config_.input_ports = input;
+
+        root = std::make_unique<Switch2>("simple_switch", simple_switch_config_);
+
+        root->addChild(&action_1);
+        root->addChild(&action_42);
+        root->addChild(&action_def);
+    }
+    ~SwitchTest()
+    {
+        root->halt();
+    }
+};
+
+TEST_F(SwitchTest, DefaultCase)
+{
+    BT::NodeStatus state = root->executeTick();
+    
+    ASSERT_EQ(NodeStatus::IDLE, action_1.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_42.status());
+    ASSERT_EQ(NodeStatus::RUNNING, action_def.status());
+    ASSERT_EQ(NodeStatus::RUNNING, state);
+
+    std::this_thread::sleep_for(milliseconds(110));
+    state = root->executeTick();
+
+    ASSERT_EQ(NodeStatus::IDLE, action_1.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_42.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_def.status());
+    ASSERT_EQ(NodeStatus::SUCCESS, state);
+}
+
+TEST_F(SwitchTest, Case1)
+{
+    bb->set("my_var", "1");
+    BT::NodeStatus state = root->executeTick();
+    
+    ASSERT_EQ(NodeStatus::RUNNING, action_1.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_42.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_def.status());
+    ASSERT_EQ(NodeStatus::RUNNING, state);
+
+    std::this_thread::sleep_for(milliseconds(110));
+    state = root->executeTick();
+
+    ASSERT_EQ(NodeStatus::IDLE, action_1.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_42.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_def.status());
+    ASSERT_EQ(NodeStatus::SUCCESS, state);
+}
+
+TEST_F(SwitchTest, Case2)
+{
+    bb->set("my_var", "42");
+    BT::NodeStatus state = root->executeTick();
+    
+    ASSERT_EQ(NodeStatus::IDLE, action_1.status());
+    ASSERT_EQ(NodeStatus::RUNNING, action_42.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_def.status());
+    ASSERT_EQ(NodeStatus::RUNNING, state);
+
+    std::this_thread::sleep_for(milliseconds(110));
+    state = root->executeTick();
+
+    ASSERT_EQ(NodeStatus::IDLE, action_1.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_42.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_def.status());
+    ASSERT_EQ(NodeStatus::SUCCESS, state);
+}
+
+TEST_F(SwitchTest, CaseNone)
+{
+    bb->set("my_var", "none");
+    BT::NodeStatus state = root->executeTick();
+    
+    ASSERT_EQ(NodeStatus::IDLE, action_1.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_42.status());
+    ASSERT_EQ(NodeStatus::RUNNING, action_def.status());
+    ASSERT_EQ(NodeStatus::RUNNING, state);
+
+    std::this_thread::sleep_for(milliseconds(110));
+    state = root->executeTick();
+
+    ASSERT_EQ(NodeStatus::IDLE, action_1.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_42.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_def.status());
+    ASSERT_EQ(NodeStatus::SUCCESS, state);
+}
+
+TEST_F(SwitchTest, CaseSwitchToDefault)
+{
+    bb->set("my_var", "1");
+    BT::NodeStatus state = root->executeTick();
+    
+    ASSERT_EQ(NodeStatus::RUNNING, action_1.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_42.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_def.status());
+    ASSERT_EQ(NodeStatus::RUNNING, state);
+
+    std::this_thread::sleep_for(milliseconds(10));
+    state = root->executeTick();
+    ASSERT_EQ(NodeStatus::RUNNING, action_1.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_42.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_def.status());
+    ASSERT_EQ(NodeStatus::RUNNING, state);
+
+    // Switch Node does not feels changes. Only when tick. 
+    // (not reactive)
+    std::this_thread::sleep_for(milliseconds(10));
+    bb->set("my_var", "");
+    std::this_thread::sleep_for(milliseconds(10));
+    ASSERT_EQ(NodeStatus::RUNNING, action_1.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_42.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_def.status());
+    ASSERT_EQ(NodeStatus::RUNNING, root->status());
+
+    std::this_thread::sleep_for(milliseconds(10));
+    state = root->executeTick();
+    ASSERT_EQ(NodeStatus::IDLE, action_1.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_42.status());
+    ASSERT_EQ(NodeStatus::RUNNING, action_def.status());
+    ASSERT_EQ(NodeStatus::RUNNING, state);
+
+    std::this_thread::sleep_for(milliseconds(110));
+    state = root->executeTick();
+
+    ASSERT_EQ(NodeStatus::IDLE, action_1.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_42.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_def.status());
+    ASSERT_EQ(NodeStatus::SUCCESS, root->status());
+}
+
+TEST_F(SwitchTest, CaseSwitchToAction2)
+{
+    bb->set("my_var", "1");
+    BT::NodeStatus state = root->executeTick();
+    
+    ASSERT_EQ(NodeStatus::RUNNING, action_1.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_42.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_def.status());
+    ASSERT_EQ(NodeStatus::RUNNING, state);
+
+    bb->set("my_var", "42");
+    std::this_thread::sleep_for(milliseconds(10));
+    state = root->executeTick();
+    ASSERT_EQ(NodeStatus::IDLE, action_1.status());
+    ASSERT_EQ(NodeStatus::RUNNING, action_42.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_def.status());
+    ASSERT_EQ(NodeStatus::RUNNING, state);
+
+    std::this_thread::sleep_for(milliseconds(110));
+    state = root->executeTick();
+
+    ASSERT_EQ(NodeStatus::IDLE, action_1.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_42.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_def.status());
+    ASSERT_EQ(NodeStatus::SUCCESS, root->status());
+}
+
+TEST_F(SwitchTest, ActionFailure)
+{
+    bb->set("my_var", "1");
+    BT::NodeStatus state = root->executeTick();
+    
+    action_1.setExpectedResult(NodeStatus::FAILURE);
+
+    ASSERT_EQ(NodeStatus::RUNNING, action_1.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_42.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_def.status());
+    ASSERT_EQ(NodeStatus::RUNNING, state);
+
+    std::this_thread::sleep_for(milliseconds(110));
+    state = root->executeTick();
+
+    ASSERT_EQ(NodeStatus::FAILURE, state);
+    ASSERT_EQ(NodeStatus::IDLE, action_1.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_42.status());
+    ASSERT_EQ(NodeStatus::IDLE, action_def.status());
+}
diff --git a/tests/gtest_tree.cpp b/tests/gtest_tree.cpp
new file mode 100644 (file)
index 0000000..ab07d05
--- /dev/null
@@ -0,0 +1,83 @@
+/* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#include <gtest/gtest.h>
+#include "action_test_node.h"
+#include "condition_test_node.h"
+#include "behaviortree_cpp_v3/behavior_tree.h"
+
+using BT::NodeStatus;
+using std::chrono::milliseconds;
+
+struct BehaviorTreeTest : testing::Test
+{
+    BT::SequenceNode root;
+    BT::AsyncActionTest action_1;
+    BT::ConditionTestNode condition_1;
+    BT::ConditionTestNode condition_2;
+
+    BT::FallbackNode fal_conditions;
+
+    BehaviorTreeTest()
+      : root("root_sequence")
+      , action_1("action_1", milliseconds(100) )
+      , condition_1("condition_1")
+      , condition_2("condition_2")
+      , fal_conditions("fallback_conditions")
+    {
+        root.addChild(&fal_conditions);
+        {
+            fal_conditions.addChild(&condition_1);
+            fal_conditions.addChild(&condition_2);
+        }
+        root.addChild(&action_1);
+    }
+    ~BehaviorTreeTest()
+    {
+    }
+};
+
+/****************TESTS START HERE***************************/
+
+TEST_F(BehaviorTreeTest, Condition1ToFalseCondition2True)
+{
+    condition_1.setExpectedResult(NodeStatus::FAILURE);
+    condition_2.setExpectedResult(NodeStatus::SUCCESS);
+
+    BT::NodeStatus state = root.executeTick();
+
+    ASSERT_EQ(NodeStatus::RUNNING, state);
+    ASSERT_EQ(NodeStatus::SUCCESS, fal_conditions.status());
+    ASSERT_EQ(NodeStatus::IDLE, condition_1.status());
+    ASSERT_EQ(NodeStatus::IDLE, condition_2.status());
+    ASSERT_EQ(NodeStatus::RUNNING, action_1.status());
+}
+
+TEST_F(BehaviorTreeTest, Condition2ToFalseCondition1True)
+{
+    condition_2.setExpectedResult(NodeStatus::FAILURE);
+    condition_1.setExpectedResult(NodeStatus::SUCCESS);
+
+    BT::NodeStatus state = root.executeTick();
+
+    ASSERT_EQ(NodeStatus::RUNNING, state);
+    ASSERT_EQ(NodeStatus::SUCCESS, fal_conditions.status());
+    ASSERT_EQ(NodeStatus::IDLE, condition_1.status());
+    ASSERT_EQ(NodeStatus::IDLE, condition_2.status());
+    ASSERT_EQ(NodeStatus::RUNNING, action_1.status());
+}
+
+int main(int argc, char** argv)
+{
+    testing::InitGoogleTest(&argc, argv);
+    return RUN_ALL_TESTS();
+}
diff --git a/tests/include/action_test_node.h b/tests/include/action_test_node.h
new file mode 100644 (file)
index 0000000..0815210
--- /dev/null
@@ -0,0 +1,81 @@
+#ifndef ACTIONTEST_H
+#define ACTIONTEST_H
+
+#include "behaviortree_cpp_v3/action_node.h"
+
+namespace BT
+{
+class SyncActionTest : public SyncActionNode
+{
+  public:
+    SyncActionTest(const std::string& name);
+
+    BT::NodeStatus tick() override;
+
+    void setExpectedResult(NodeStatus res);
+
+    int tickCount() const
+    {
+        return tick_count_;
+    }
+
+    void resetTicks()
+    {
+        tick_count_ = 0;
+    }
+
+  private:
+    NodeStatus expected_result_;
+    int tick_count_;
+};
+
+class AsyncActionTest : public AsyncActionNode
+{
+  public:
+    AsyncActionTest(const std::string& name, BT::Duration deadline_ms = std::chrono::milliseconds(100) );
+
+    virtual ~AsyncActionTest()
+    {
+        halt();
+    }
+
+    // The method that is going to be executed by the thread
+    BT::NodeStatus tick() override;
+
+    void setTime(BT::Duration time);
+
+    // The method used to interrupt the execution of the node
+    virtual void halt() override;
+
+    void setExpectedResult(NodeStatus res);
+
+    int tickCount() const {
+        return tick_count_;
+    }
+
+    int successCount() const {
+        return success_count_;
+    }
+
+    int failureCount() const {
+        return failure_count_;
+    }
+
+    void resetCounters() {
+        success_count_ = 0;
+        failure_count_ = 0;
+        tick_count_ = 0;
+    }
+
+  private:
+    // using atomic because these variables might be accessed from different threads
+    BT::Duration time_;
+    std::atomic<NodeStatus> expected_result_;
+    std::atomic<int> tick_count_;
+    int success_count_;
+    int failure_count_;
+
+};
+}
+
+#endif
diff --git a/tests/include/condition_test_node.h b/tests/include/condition_test_node.h
new file mode 100644 (file)
index 0000000..0308050
--- /dev/null
@@ -0,0 +1,30 @@
+#ifndef CONDITIONTEST_H
+#define CONDITIONTEST_H
+
+#include "behaviortree_cpp_v3/condition_node.h"
+
+namespace BT
+{
+class ConditionTestNode : public ConditionNode
+{
+  public:
+    
+    ConditionTestNode(const std::string& name);
+
+    void setExpectedResult(NodeStatus res);
+
+    // The method that is going to be executed by the thread
+    virtual BT::NodeStatus tick() override;
+
+    int tickCount() const
+    {
+        return tick_count_;
+    }
+
+  private:
+    NodeStatus expected_result_;
+    int tick_count_;
+};
+}
+
+#endif
diff --git a/tests/navigation_test.cpp b/tests/navigation_test.cpp
new file mode 100644 (file)
index 0000000..71787d7
--- /dev/null
@@ -0,0 +1,309 @@
+#include "behaviortree_cpp_v3/xml_parsing.h"
+#include "behaviortree_cpp_v3/blackboard.h"
+#include <gtest/gtest.h>
+
+using namespace BT;
+
+// clang-format off
+static const char* xml_text = R"(
+
+<root main_tree_to_execute="BehaviorTree">
+    <BehaviorTree ID="BehaviorTree">
+        <Fallback name="root">
+
+            <ReactiveSequence name="navigation_subtree">
+                <Inverter>
+                    <Condition ID="IsStuck"/>
+                </Inverter>
+                <SequenceStar name="navigate">
+                    <Action ID="ComputePathToPose"/>
+                    <Action ID="FollowPath"/>
+                </SequenceStar>
+            </ReactiveSequence>
+
+            <SequenceStar name="stuck_recovery">
+                <Condition ID="IsStuck"/>
+                <Action ID="BackUpAndSpin"/>
+            </SequenceStar>
+
+        </Fallback>
+    </BehaviorTree>
+</root>
+ )";
+
+// clang-format on
+
+using Milliseconds = std::chrono::milliseconds;
+
+inline std::chrono::high_resolution_clock::time_point Now()
+{
+    return std::chrono::high_resolution_clock::now();
+}
+
+
+
+//--------------------------------------------
+
+class TestNode
+{
+  public:
+    TestNode(const std::string& name):
+      _expected_result(true),
+      _tick_count(0),
+      _name(name)
+    { }
+
+    void setExpectedResult(bool will_succeed)
+    {
+        _expected_result = will_succeed;
+    }
+    NodeStatus expectedResult() const
+    {
+        return _expected_result ? NodeStatus::SUCCESS : NodeStatus::FAILURE;
+    }
+    void resetTickCount() { _tick_count = 0; }
+    int tickCount() const { return _tick_count;}
+
+    NodeStatus tickImpl()
+    {
+        std::cout << _name << ": "<< (_expected_result? "true" : "false") << std::endl;
+        _tick_count++;
+        return expectedResult();
+    }
+
+  private:
+    bool _expected_result;
+    int _tick_count;
+    std::string _name;
+};
+
+class IsStuck: public ConditionNode, public TestNode
+{
+  public:
+    IsStuck(const std::string& name): ConditionNode(name, {}), TestNode(name) {}
+
+    NodeStatus tick() override
+    {
+        return tickImpl();
+    }
+};
+
+class BackUpAndSpin: public SyncActionNode, public TestNode
+{
+  public:
+    BackUpAndSpin(const std::string& name): SyncActionNode(name, {}), TestNode(name){}
+
+    NodeStatus tick() override
+    {
+        return tickImpl();
+    }
+};
+
+class ComputePathToPose: public SyncActionNode, public TestNode
+{
+  public:
+    ComputePathToPose(const std::string& name): SyncActionNode(name, {}), TestNode(name){}
+
+    NodeStatus tick() override
+    {
+        return tickImpl();
+    }
+};
+
+class FollowPath: public ActionNodeBase, public TestNode
+{
+    std::chrono::high_resolution_clock::time_point _initial_time;
+  public:
+    FollowPath(const std::string& name): ActionNodeBase(name, {}), TestNode(name), _halted(false){}
+
+    NodeStatus tick() override
+    {
+        if( status() == NodeStatus::IDLE )
+        {
+            setStatus(NodeStatus::RUNNING);
+            _halted = false;
+            std::cout << "FollowPath::started" << std::endl;
+            _initial_time = Now();
+        }
+
+        // Yield for 1 second
+        while( Now() < _initial_time + Milliseconds(600) || _halted )
+        {
+            return NodeStatus::RUNNING;
+        }
+        if( _halted )
+        {
+            return NodeStatus::IDLE;
+        }
+        return tickImpl();
+    }
+    void halt() override {
+        std::cout << "FollowPath::halt" << std::endl;
+        _halted = true;
+    }
+
+    bool wasHalted() const { return _halted; }
+
+  private:
+    bool _halted;
+};
+
+//-------------------------------------
+
+template <typename Original, typename Casted>
+void TryDynamicCastPtr(Original* ptr, Casted*& destination)
+{
+    if( dynamic_cast<Casted*>(ptr) )
+    {
+        destination = dynamic_cast<Casted*>(ptr);
+    }
+}
+
+/****************TESTS START HERE***************************/
+
+TEST(Navigationtest, MoveBaseRecovery)
+{
+    BehaviorTreeFactory factory;
+
+    factory.registerNodeType<IsStuck>("IsStuck");
+    factory.registerNodeType<BackUpAndSpin>("BackUpAndSpin");
+    factory.registerNodeType<ComputePathToPose>("ComputePathToPose");
+    factory.registerNodeType<FollowPath>("FollowPath");
+
+    auto tree = factory.createTreeFromText(xml_text);
+
+    // Need to retrieve the node pointers with dynamic cast
+    // In a normal application you would NEVER want to do such a thing.
+    IsStuck *first_stuck_node = nullptr;
+    IsStuck *second_stuck_node = nullptr;
+    BackUpAndSpin* back_spin_node = nullptr;
+    ComputePathToPose* compute_node = nullptr;
+    FollowPath* follow_node = nullptr;
+
+    for (auto& node: tree.nodes)
+    {
+        auto ptr = node.get();
+
+        if( !first_stuck_node )
+        {
+            TryDynamicCastPtr(ptr, first_stuck_node);
+        }
+        else{
+            TryDynamicCastPtr(ptr, second_stuck_node);
+        }
+        TryDynamicCastPtr(ptr, back_spin_node);
+        TryDynamicCastPtr(ptr, follow_node);
+        TryDynamicCastPtr(ptr, compute_node);
+    }
+
+    ASSERT_TRUE( first_stuck_node );
+    ASSERT_TRUE( second_stuck_node );
+    ASSERT_TRUE( back_spin_node );
+    ASSERT_TRUE( compute_node );
+    ASSERT_TRUE( follow_node );
+
+    std::cout << "-----------------------" << std::endl;
+
+    // First case: not stuck, everything fine.
+    NodeStatus status = NodeStatus::IDLE;
+
+    first_stuck_node->setExpectedResult(false);
+
+    while( status == NodeStatus::IDLE || status == NodeStatus::RUNNING )
+    {
+        status = tree.tickRoot();
+        std::this_thread::sleep_for(Milliseconds(100));
+    }
+
+    // SUCCESS expected
+    ASSERT_EQ( status,  NodeStatus::SUCCESS );
+    // IsStuck on the left branch must run several times
+    ASSERT_GE( first_stuck_node->tickCount(), 6);
+    // Never take the right branch (recovery)
+    ASSERT_EQ( second_stuck_node->tickCount(), 0 );
+    ASSERT_EQ( back_spin_node->tickCount(), 0 );
+
+    ASSERT_EQ( compute_node->tickCount(), 1 );
+    ASSERT_EQ( follow_node->tickCount(), 1 );
+    ASSERT_FALSE( follow_node->wasHalted() );
+
+    std::cout << "-----------------------" << std::endl;
+
+    // Second case: get stuck after a while
+
+    // Initialize evrything first
+    first_stuck_node->resetTickCount();
+    second_stuck_node->resetTickCount();
+    compute_node->resetTickCount();
+    follow_node->resetTickCount();
+    back_spin_node->resetTickCount();
+    status = NodeStatus::IDLE;
+    int cycle = 0;
+
+    while( status == NodeStatus::IDLE || status == NodeStatus::RUNNING )
+    {
+        // At the fifth cycle get stucked
+        if( ++cycle == 2 )
+        {
+            first_stuck_node->setExpectedResult(true);
+            second_stuck_node->setExpectedResult(true);
+        }
+        status = tree.tickRoot();
+        std::this_thread::sleep_for(Milliseconds(100));
+    }
+
+    // SUCCESS expected
+    ASSERT_EQ( status,  NodeStatus::SUCCESS );
+
+    // First IsStuck must run several times
+    ASSERT_GE( first_stuck_node->tickCount(), 2);
+    // Second IsStuck probably only once
+    ASSERT_EQ( second_stuck_node->tickCount(), 1 );
+    ASSERT_EQ( back_spin_node->tickCount(), 1 );
+
+    // compute done once and follow started but halted
+    ASSERT_EQ( compute_node->tickCount(), 1 );
+
+    ASSERT_EQ( follow_node->tickCount(), 0 ); // started but never completed
+    ASSERT_TRUE( follow_node->wasHalted() );
+
+    ASSERT_EQ( compute_node->status(),  NodeStatus::IDLE );
+    ASSERT_EQ( follow_node->status(),  NodeStatus::IDLE );
+    ASSERT_EQ( back_spin_node->status(),  NodeStatus::IDLE );
+
+
+    std::cout << "-----------------------" << std::endl;
+
+    // Third case: execute again
+
+    // Initialize everything first
+    first_stuck_node->resetTickCount();
+    second_stuck_node->resetTickCount();
+    compute_node->resetTickCount();
+    follow_node->resetTickCount();
+    back_spin_node->resetTickCount();
+    status = NodeStatus::IDLE;
+    first_stuck_node->setExpectedResult(false);
+    second_stuck_node->setExpectedResult(false);
+
+    while( status == NodeStatus::IDLE || status == NodeStatus::RUNNING )
+    {
+        status = tree.tickRoot();
+        std::this_thread::sleep_for(Milliseconds(100));
+    }
+
+    // SUCCESS expected
+    ASSERT_EQ( status,  NodeStatus::SUCCESS );
+
+    ASSERT_GE( first_stuck_node->tickCount(), 6);
+    ASSERT_EQ( second_stuck_node->tickCount(), 0 );
+    ASSERT_EQ( back_spin_node->tickCount(), 0 );
+
+    ASSERT_EQ( compute_node->status(),  NodeStatus::IDLE );
+    ASSERT_EQ( follow_node->status(),  NodeStatus::IDLE );
+    ASSERT_EQ( back_spin_node->status(),  NodeStatus::IDLE );
+    ASSERT_FALSE( follow_node->wasHalted() );
+
+}
+
+
diff --git a/tests/src/action_test_node.cpp b/tests/src/action_test_node.cpp
new file mode 100644 (file)
index 0000000..d18c8ca
--- /dev/null
@@ -0,0 +1,89 @@
+/* Copyright (C) 2015-2017 Michele Colledanchise -  All Rights Reserved
+ * Copyright (C) 2018-2019 Davide Faconti, Eurecat -  All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#include "action_test_node.h"
+#include <string>
+
+BT::AsyncActionTest::AsyncActionTest(const std::string& name, BT::Duration deadline_ms) :
+    AsyncActionNode(name, {}),
+  success_count_(0),
+  failure_count_(0)
+{
+    expected_result_ = NodeStatus::SUCCESS;
+    time_ = deadline_ms;
+    tick_count_ = 0;
+}
+
+BT::NodeStatus BT::AsyncActionTest::tick()
+{
+    using std::chrono::high_resolution_clock;
+    tick_count_++;
+
+    auto initial_time = high_resolution_clock::now();
+
+    // we simulate an asynchronous action that takes an amount of time equal to time_
+    while (!isHaltRequested() && high_resolution_clock::now() < initial_time + time_)
+    {
+        std::this_thread::sleep_for(std::chrono::milliseconds(1));
+    }
+
+    // check if we exited the while(9 loop because of the flag stop_loop_
+    if( isHaltRequested() ){
+        return NodeStatus::IDLE;
+    }
+
+    if( expected_result_ == NodeStatus::SUCCESS){
+        success_count_++;
+    }
+    else if( expected_result_ == NodeStatus::FAILURE){
+        failure_count_++;
+    }
+
+    return expected_result_;
+}
+
+void BT::AsyncActionTest::halt()
+{
+    // do more cleanup here if necessary
+    AsyncActionNode::halt();
+}
+
+void BT::AsyncActionTest::setTime(BT::Duration time)
+{
+    time_ = time;
+}
+
+void BT::AsyncActionTest::setExpectedResult(BT::NodeStatus res)
+{
+    expected_result_ = res;
+}
+
+//----------------------------------------------
+
+BT::SyncActionTest::SyncActionTest(const std::string& name) :
+    SyncActionNode(name, {})
+{
+    tick_count_ = 0;
+    expected_result_ = NodeStatus::SUCCESS;
+}
+
+BT::NodeStatus BT::SyncActionTest::tick()
+{
+    tick_count_++;
+    return expected_result_;
+}
+
+void BT::SyncActionTest::setExpectedResult(NodeStatus res)
+{
+    expected_result_ = res;
+}
diff --git a/tests/src/condition_test_node.cpp b/tests/src/condition_test_node.cpp
new file mode 100644 (file)
index 0000000..98f4b8d
--- /dev/null
@@ -0,0 +1,32 @@
+/* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#include "condition_test_node.h"
+#include <string>
+
+BT::ConditionTestNode::ConditionTestNode(const std::string& name)
+    : ConditionNode::ConditionNode(name, {})
+{
+    expected_result_ = NodeStatus::SUCCESS;
+    tick_count_ = 0;
+}
+
+BT::NodeStatus BT::ConditionTestNode::tick()
+{
+    tick_count_++;
+    return expected_result_;
+}
+
+void BT::ConditionTestNode::setExpectedResult(NodeStatus res)
+{
+    expected_result_ = res;
+}
diff --git a/tools/CMakeLists.txt b/tools/CMakeLists.txt
new file mode 100644 (file)
index 0000000..0801850
--- /dev/null
@@ -0,0 +1,22 @@
+cmake_minimum_required(VERSION 2.8)
+
+
+add_executable(bt3_log_cat         bt_log_cat.cpp )
+target_link_libraries(bt3_log_cat  ${BEHAVIOR_TREE_LIBRARY} )
+install(TARGETS bt3_log_cat
+        DESTINATION ${BEHAVIOR_TREE_BIN_DESTINATION} )
+
+if( ZMQ_FOUND )
+    add_executable(bt3_recorder         bt_recorder.cpp )
+    target_link_libraries(bt3_recorder  ${BEHAVIOR_TREE_LIBRARY} ${ZMQ_LIBRARIES})
+    install(TARGETS bt3_recorder
+            DESTINATION ${BEHAVIOR_TREE_BIN_DESTINATION} )
+endif()
+
+add_executable(bt3_plugin_manifest         bt_plugin_manifest.cpp )
+target_link_libraries(bt3_plugin_manifest  ${BEHAVIOR_TREE_LIBRARY} )
+install(TARGETS bt3_plugin_manifest
+        DESTINATION ${BEHAVIOR_TREE_BIN_DESTINATION} )
+
+
+
diff --git a/tools/bt_log_cat.cpp b/tools/bt_log_cat.cpp
new file mode 100644 (file)
index 0000000..8822d44
--- /dev/null
@@ -0,0 +1,108 @@
+#include <stdio.h>
+#include <iostream>
+#include <fstream>
+#include <unordered_map>
+#include "behaviortree_cpp_v3/flatbuffers/BT_logger_generated.h"
+
+int main(int argc, char* argv[])
+{
+    if (argc != 2)
+    {
+        printf("Wrong number of arguments\nUsage: %s [filename]\n", argv[0]);
+        return 1;
+    }
+
+    FILE* file = fopen(argv[1], "rb");
+
+    if (!file)
+    {
+        printf("Failed to open file: [%s]\n", argv[1]);
+        return 1;
+    }
+
+    fseek(file, 0L, SEEK_END);
+    const size_t length = ftell(file);
+    fseek(file, 0L, SEEK_SET);
+    std::vector<char> buffer(length);
+    fread(buffer.data(), sizeof(char), length, file);
+    fclose(file);
+
+    const int bt_header_size = flatbuffers::ReadScalar<uint32_t>(&buffer[0]);
+
+    auto behavior_tree = Serialization::GetBehaviorTree(&buffer[4]);
+
+    std::unordered_map<uint16_t, std::string> names_by_uid;
+    std::unordered_map<uint16_t, const Serialization::TreeNode*> node_by_uid;
+
+    for (const Serialization::TreeNode* node : *(behavior_tree->nodes()))
+    {
+        names_by_uid.insert({node->uid(), std::string(node->instance_name()->c_str())});
+        node_by_uid.insert({node->uid(), node});
+    }
+
+    printf("----------------------------\n");
+
+    std::function<void(uint16_t, int)> recursiveStep;
+
+    recursiveStep = [&](uint16_t uid, int indent) {
+        for (int i = 0; i < indent; i++)
+        {
+            printf("    ");
+            names_by_uid[uid] = std::string("   ") + names_by_uid[uid];
+        }
+        printf("%s\n", names_by_uid[uid].c_str());
+        std::cout << std::flush;
+
+        const auto& node = node_by_uid[uid];
+
+        for (size_t i = 0; i < node->children_uid()->size(); i++)
+        {
+            recursiveStep(node->children_uid()->Get(i), indent + 1);
+        }
+    };
+
+    recursiveStep(behavior_tree->root_uid(), 0);
+
+    printf("----------------------------\n");
+
+    constexpr const char* whitespaces = "                         ";
+    constexpr const size_t ws_count = 25;
+
+    auto printStatus = [](Serialization::NodeStatus status) {
+        switch (status)
+        {
+            case Serialization::NodeStatus::SUCCESS:
+                return ("\x1b[32m"
+                        "SUCCESS"
+                        "\x1b[0m");   // RED
+            case Serialization::NodeStatus::FAILURE:
+                return ("\x1b[31m"
+                        "FAILURE"
+                        "\x1b[0m");   // GREEN
+            case Serialization::NodeStatus::RUNNING:
+                return ("\x1b[33m"
+                        "RUNNING"
+                        "\x1b[0m");   // YELLOW
+            case Serialization::NodeStatus::IDLE:
+                return ("\x1b[36m"
+                        "IDLE   "
+                        "\x1b[0m");   // CYAN
+        }
+        return "Undefined";
+    };
+
+    for (size_t index = bt_header_size + 4; index < length; index += 12)
+    {
+        const uint16_t uid = flatbuffers::ReadScalar<uint16_t>(&buffer[index + 8]);
+        const std::string& name = names_by_uid[uid];
+        const uint32_t t_sec = flatbuffers::ReadScalar<uint32_t>(&buffer[index]);
+        const uint32_t t_usec = flatbuffers::ReadScalar<uint32_t>(&buffer[index + 4]);
+
+        printf("[%d.%06d]: %s%s %s -> %s\n", t_sec, t_usec, name.c_str(),
+               &whitespaces[std::min(ws_count, name.size())],
+               printStatus(flatbuffers::ReadScalar<Serialization::NodeStatus>(&buffer[index + 10])),
+               printStatus(flatbuffers::ReadScalar<Serialization::NodeStatus>(&buffer[index + 11])));
+    }
+
+    return 0;
+}
diff --git a/tools/bt_plugin_manifest.cpp b/tools/bt_plugin_manifest.cpp
new file mode 100644 (file)
index 0000000..27e2dc4
--- /dev/null
@@ -0,0 +1,52 @@
+#include <stdio.h>
+#include <iostream>
+#include <fstream>
+#include <unordered_map>
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+int main(int argc, char* argv[])
+{
+    if (argc != 2)
+    {
+        printf("Wrong number of command line arguments\nUsage: %s [filename]\n", argv[0]);
+        return 1;
+    }
+
+    BT::BehaviorTreeFactory factory;
+
+    std::unordered_set<std::string> default_nodes;
+    for (auto& it : factory.manifests())
+    {
+        const auto& manifest = it.second;
+        default_nodes.insert(manifest.registration_ID);
+    }
+
+    factory.registerFromPlugin(argv[1]);
+
+    for (auto& it : factory.manifests())
+    {
+        const auto& manifest = it.second;
+        if (default_nodes.count(manifest.registration_ID) > 0)
+        {
+            continue;
+        }
+        auto& params = manifest.ports;
+        std::cout << "---------------\n"
+                  << manifest.registration_ID << " [" << manifest.type
+                  << "]\n  NodeParameters: " << params.size();
+
+        if (params.size() > 0)
+        {
+            std::cout << ":";
+        }
+
+        std::cout << std::endl;
+
+        for (auto& param : params)
+        {
+            std::cout << "    - [Key]: \"" << param.first << "\"" << std::endl;
+        }
+    }
+
+    return 0;
+}
diff --git a/tools/bt_recorder.cpp b/tools/bt_recorder.cpp
new file mode 100644 (file)
index 0000000..3aa6740
--- /dev/null
@@ -0,0 +1,99 @@
+#include <stdio.h>
+#include <iostream>
+#include <fstream>
+#include <signal.h>
+#include <zmq.hpp>
+#include <fstream>
+#include "behaviortree_cpp_v3/flatbuffers/BT_logger_generated.h"
+
+// http://zguide.zeromq.org/cpp:interrupt
+static bool s_interrupted = false;
+
+static void s_signal_handler(int)
+{
+    s_interrupted = true;
+}
+
+static void CatchSignals(void)
+{
+    struct sigaction action;
+    action.sa_handler = s_signal_handler;
+    action.sa_flags = 0;
+    sigemptyset(&action.sa_mask);
+    sigaction(SIGINT, &action, NULL);
+    sigaction(SIGTERM, &action, NULL);
+}
+
+int main(int argc, char* argv[])
+{
+    if (argc != 2)
+    {
+        printf("Wrong number of arguments\nUsage: %s [filename]\n", argv[0]);
+        return 1;
+    }
+
+    // register CTRL+C signal handler
+    CatchSignals();
+
+    zmq::context_t context(1);
+
+    //  Socket to talk to server
+    std::cout << "Trying to connect to [tcp://localhost:1666]\n" << std::endl;
+
+    zmq::socket_t subscriber(context, ZMQ_SUB);
+    subscriber.connect("tcp://localhost:1666");
+
+    //  Subscribe to everything
+    subscriber.setsockopt(ZMQ_SUBSCRIBE, "", 0);
+
+    printf("----------- Started -----------------\n");
+
+    bool first_message = true;
+    std::ofstream file_os;
+
+    while (!s_interrupted)
+    {
+        zmq::message_t update;
+        zmq::message_t msg;
+        try
+        {
+            subscriber.recv(&update, 0);
+        }
+        catch (zmq::error_t& e)
+        {
+            if (!s_interrupted)
+            {
+                std::cout << "subscriber.recv() failed with exception: " << e.what() << std::endl;
+                return -1;
+            }
+        }
+
+        if (!s_interrupted)
+        {
+            char* data_ptr = static_cast<char*>(update.data());
+            const uint32_t header_size = flatbuffers::ReadScalar<uint32_t>(data_ptr);
+
+            if (first_message)
+            {
+                printf("First message received\n");
+                first_message = false;
+
+                file_os.open(argv[1], std::ofstream::binary | std::ofstream::out);
+                file_os.write(data_ptr, 4 + header_size);
+            }
+            data_ptr += 4 + header_size;
+
+            const uint32_t transition_count = flatbuffers::ReadScalar<uint32_t>(data_ptr);
+            data_ptr += sizeof(uint32_t);
+
+            file_os.write(data_ptr, 12 * transition_count);
+        }
+    }
+
+    subscriber.close();
+
+    printf("Results saved to file\n");
+    file_os.close();
+
+    return 0;
+}