diff --git a/.github/workflows/ros1.yaml b/.github/workflows/ros1.yaml index 524552c36..06bbbb1a1 100644 --- a/.github/workflows/ros1.yaml +++ b/.github/workflows/ros1.yaml @@ -7,7 +7,6 @@ jobs: strategy: matrix: env: - - {ROS_DISTRO: melodic, ROS_REPO: main} - {ROS_DISTRO: noetic, ROS_REPO: main} runs-on: ubuntu-latest steps: diff --git a/.github/workflows/ros2.yaml b/.github/workflows/ros2.yaml index 9f8c5e764..f55d0be9b 100644 --- a/.github/workflows/ros2.yaml +++ b/.github/workflows/ros2.yaml @@ -7,7 +7,7 @@ jobs: strategy: matrix: env: - - {ROS_DISTRO: eloquent, ROS_REPO: main} + - {ROS_DISTRO: humble, ROS_REPO: main} runs-on: ubuntu-latest steps: - uses: actions/checkout@v1 diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 0b4d8f2bd..ff31395d5 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,6 +2,153 @@ Changelog for package behaviortree_cpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.8.7 (2024-06-26) +------------------ +* Backport of some build-related flatbuffers changes (`#825 `_) + * From flatbuffers upstream: Fix compiler error + Original author of change: avaliente-bc + Backport/update from upstream flatbuffers repository. + Change taken from https://github.com/google/flatbuffers/pull/7227 + * From flatbuffers upstream: Fix include of string_view with C++17 abseil + Original author of change: ocpalo + Backport/update from upstream flatbuffers repository. + Changes taken from https://github.com/google/flatbuffers/pull/7897. +* Add in call to ament_export_targets. (`#826 `_) + That way downstream ament packages can use this + package as a CMake target. +* Fixed `#810 `_ - halting of subsequent nodes in ReactiveSequence/Fallback (`#817 `_) + * ReactiveSequence and ReactiveFallback will behave more similarly to 3.8 + * Reactive Sequence/Fallback defaulting to allow multiple async nodes + --------- + Co-authored-by: Davide Faconti + Co-authored-by: Matej Vargovcik +* Merge pull request `#769 `_ from bi0ha2ard/fewer_boost_dependencies + depend only on libboost-coroutine(-dev) for v3.8 +* fix(dependency): depend only on libboost-coroutine(-dev) + At least on Ubuntu, boost-all-dev depends on openmpi, which depends on a + fortran compiler and gcc. This is very heavy for Docker containers where + only exec dependencies are really needed. +* alternative to `#719 `_ +* fix issue `#725 `_ : SetBlackboard can copy entries +* Contributors: Chris Lalancette, Davide Faconti, Felix, Lars Toenning, afrixs + +3.8.5 (2023-08-14) +------------------ + +3.8.4 (2023-06-28) +------------------ +* Update ros2.yaml +* Update ros1.yaml +* Issue 563 (`#596 `_) + * failing test + * fix issue 563 (?) + * better solution +* use lambda in tutorial +* Merge pull request `#583 `_ from BehaviorTree/issue563 + Issue563 +* better default port +* restore type check +* fix issue `#563 `_ +* fix test +* Issue563 +* Merge pull request `#579 `_ from open-navigation/hi + changing resetStatus to public +* Update tree_node.h +* changing resetStatus to public +* Merge branch 'v3.8' of github.com:BehaviorTree/BehaviorTree.CPP into v3.8 +* backporting fixes from branch 4.x +* Merge pull request `#546 `_ from divbyzerofordummies/fix_ROS_include + Fix issue `#545 `_ +* Fix issue `#545 `_ +* bug fix: halting a Node must invoke the Loggers +* unit test added +* Contributors: Daniel Muschick, Davide Faconti, Steve Macenski, stevemacenski + +3.8.3 (2023-03-01) +------------------ +* fix and warnings added +* fix in SharedLibrary and cosmetic changes to the code +* Contributors: Davide Faconti + +3.8.2 (2023-01-05) +------------------ +* rebane haltChildren to resetChildren +* revert `#329 `_ +* Contributors: Davide Faconti + +3.8.1 (2022-11-27) +------------------ +* fix catkin installation `#478 `_ +* cherry picking changes from v4 +* fix `#227 `_ +* fix issue `#461 `_ +* fix issue `#413 `_ (Delay logic) +* Update README.md +* Contributors: Davide Faconti + +3.8.0 (2022-10-11) +------------------ +* tickRootWhileRunning method +* Fix: PublisherZMQ::flush is called after the publisher has been destructed (`#426 `_) + * fix: PublisherZMQ::flush is called after the publisher has been destructed + * style: Adjust code formatting of ~PublisherZMQ + * chore: Install zmq-dev in ubuntu pipeline and exclude gtest_logger_zmq.cpp when zmq is not found. + * chore: Define WIN32_LEAN_AND_MEAN to avoid ambiguity between tinyxml and msxml +* fix missing closing brace in unit test (`#442 `_) +* Fix incorrect registration of behavior trees containing faulty XML (`#438 `_) + * fix incorrect registration of faulty trees + * format + * simplify XML validation + * fix possible out-of-range exception in tests + * Add tests + * reduce scale of diffs + * fix comment + * add more test cases + Co-authored-by: Davide Faconti +* Add functionality to clear registered behavior trees. (`#439 `_) + Co-authored-by: Jere Liukkonen +* Wait for the thread to finish before deleting zmq (`#440 `_) + Co-authored-by: JafarAbdi +* clang form at +* clang format +* new clang format +* Moving tinyxml2 to 3rdparty +* Merge branch 'master' of github.com:BehaviorTree/BehaviorTree.CPP +* backporting changes from v4.x +* Update README.md +* fix warnings +* Merge branch 'master' of github.com:BehaviorTree/BehaviorTree.CPP +* fix issue `#433 `_ +* Added ros_environment dependency to make sure ROS_VERSION is initialized (`#420 `_) +* Added XML validation for decorators without children (`#424 `_) + * Added unit tests to demonstrate failure + * Added validation that decorators have only one child +* Update expected-lite to 0.6.2 (`#418 `_) +* fix test +* parallel node fix +* threshold child count dynamically in parallel control node (`#363 `_) +* Adding the reserved word "_description" (`#394 `_) +* fix(README): change find_package() instruction for BT external usage (`#401 `_) + Co-authored-by: Luca Bonamini +* Example suggests it's not restricted to a few (`#414 `_) + * Example suggests it's not restricted to a few + * Update delay_node.h + Fix flow of sentence, milliseconds is already put in specification. +* documentation and doc correction +* Merge branch 'master' of github.com:BehaviorTree/BehaviorTree.CPP +* improve writeTreeNodesModelXML +* Shutdown zmq context after joining the server thread and flushing (`#400 `_) +* Update README.md +* add option to conditionally build manual selector node (`#397 `_) + * add option to conditionally build manual selector node + * do not fail if BUILD_MANUAL_SELECTOR is true but Curses is not found +* remove variables that depend on CMAKE_BINARY_DIR being set (`#398 `_) + * remove variables that depend on CMAKE_BINARY_DIR being set + * Update cmake.yml +* Small comments on node registration (`#399 `_) +* Fix destination in CMakeLists.txt (`#389 `_) +* Contributors: Adam Sasine, Alberto Soragna, AndyZe, Davide Faconti, Dennis, Gaël Écorchard, Jafar, Joseph Schornak, Luca Bonamini, Paul Bovbel, Tim Clephas, Will + 3.7.0 (2022-05-23) ----------- * add netlify stuff diff --git a/CMakeLists.txt b/CMakeLists.txt index 1a84889de..334d6578e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -19,7 +19,6 @@ set(CMAKE_POSITION_INDEPENDENT_CODE ON) #---- project configuration ---- option(BUILD_EXAMPLES "Build tutorials and examples" ON) -option(BUILD_SAMPLES "Build sample nodes" 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) @@ -35,23 +34,22 @@ if(ENABLE_COROUTINES) if(NOT Boost_VERSION_NODOT VERSION_LESS 105900) message(STATUS "Found boost::coroutine2.") add_definitions(-DBT_BOOST_COROUTINE2) - set(BT_COROUTINES true) + set(BT_COROUTINES_FOUND true) elseif(NOT Boost_VERSION_NODOT VERSION_LESS 105300) message(STATUS "Found boost::coroutine.") add_definitions(-DBT_BOOST_COROUTINE) - set(BT_COROUTINES true) + set(BT_COROUTINES_FOUND true) endif() - include_directories(${Boost_INCLUDE_DIRS}) endif() - if(NOT DEFINED BT_COROUTINES) + if(NOT DEFINED BT_COROUTINES_FOUND) message(STATUS "Boost coroutines disabled. Install Boost (version 1.59+ recommended).") endif() else() message(STATUS "Boost coroutines disabled by CMake option.") endif() -if(NOT DEFINED BT_COROUTINES) +if(NOT DEFINED BT_COROUTINES_FOUND) add_definitions(-DBT_NO_COROUTINES) endif() @@ -59,13 +57,8 @@ endif() 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.") + message(STATUS "ZeroMQ: found.") add_definitions( -DZMQ_FOUND ) list(APPEND BT_SOURCE src/loggers/bt_zmq_publisher.cpp) else() @@ -89,8 +82,6 @@ if ( ament_cmake_FOUND ) 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) @@ -108,9 +99,6 @@ elseif( CATKIN_DEVEL_PREFIX OR CATKIN_BUILD_BINARY_PACKAGE) CATKIN_DEPENDS roslib ) - list(APPEND BEHAVIOR_TREE_PUBLIC_LIBRARIES ${catkin_LIBRARIES}) - set(BUILD_TOOL_INCLUDE_DIRS ${catkin_INCLUDE_DIRS}) - elseif(BUILD_UNIT_TESTS) if(${CMAKE_VERSION} VERSION_LESS "3.11.0") find_package(GTest REQUIRED) @@ -170,11 +158,8 @@ list(APPEND BT_SOURCE if(BUILD_MANUAL_SELECTOR) 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) + message(STATUS "NCurses: found.") + list(APPEND BT_SOURCE src/controls/manual_node.cpp ) else() message(WARNING "NCurses NOT found. Skipping the build of manual selector node.") endif() @@ -199,34 +184,47 @@ else() 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} + PUBLIC + ${CMAKE_THREAD_LIBS_INIT} + ${CMAKE_DL_LIBS} +) -target_link_libraries(${BEHAVIOR_TREE_LIBRARY} PRIVATE - ${Boost_LIBRARIES} - ${ZMQ_LIBRARIES}) +target_include_directories(${BEHAVIOR_TREE_LIBRARY} + PUBLIC + $ + $ + PRIVATE + $ + $ + $ + ) -#get_target_property(my_libs ${BEHAVIOR_TREE_LIBRARY} INTERFACE_LINK_LIBRARIES) -#list(REMOVE_ITEM _libs X) -#message("my_libs: ${my_libs}") +if(ZMQ_FOUND) + target_link_libraries(${BEHAVIOR_TREE_LIBRARY} PRIVATE + ${ZMQ_LIBRARIES} ) + target_include_directories(${BEHAVIOR_TREE_LIBRARY} PRIVATE + ${ZMQ_INCLUDE_DIRS} ) + target_compile_definitions(${BEHAVIOR_TREE_LIBRARY} PRIVATE ZMQ_FOUND) +endif() -#set_target_properties(${BEHAVIOR_TREE_LIBRARY} PROPERTIES INTERFACE_LINK_LIBRARIES "") +if(BT_COROUTINES_FOUND) + target_link_libraries(${BEHAVIOR_TREE_LIBRARY} PRIVATE + ${Boost_LIBRARIES} ) + target_include_directories(${BEHAVIOR_TREE_LIBRARY} PRIVATE + ${Boost_INCLUDE_DIRS} ) +endif() -target_compile_definitions(${BEHAVIOR_TREE_LIBRARY} PRIVATE $<$:TINYXML2_DEBUG>) +if(CURSES_FOUND) + target_link_libraries(${BEHAVIOR_TREE_LIBRARY} PRIVATE + ${CURSES_LIBRARIES} ) + target_include_directories(${BEHAVIOR_TREE_LIBRARY} PRIVATE + ${CURSES_INCLUDE_DIRS} ) + target_compile_definitions(${BEHAVIOR_TREE_LIBRARY} PRIVATE NCURSES_FOUND) +endif() -target_include_directories(${BEHAVIOR_TREE_LIBRARY} PUBLIC - $ - $ - $ - ${BUILD_TOOL_INCLUDE_DIRS}) -if( ZMQ_FOUND ) - target_compile_definitions(${BEHAVIOR_TREE_LIBRARY} PUBLIC ZMQ_FOUND) -endif() +target_compile_definitions(${BEHAVIOR_TREE_LIBRARY} PRIVATE $<$:TINYXML2_DEBUG>) if(MSVC) else() @@ -237,7 +235,13 @@ endif() ############################################################# if(ament_cmake_FOUND) find_package(ament_index_cpp REQUIRED) - ament_target_dependencies(${BEHAVIOR_TREE_LIBRARY} PUBLIC ament_index_cpp) + + target_include_directories(${BEHAVIOR_TREE_LIBRARY} PRIVATE + $ ) + + target_link_libraries(${BEHAVIOR_TREE_LIBRARY} PRIVATE + $ ) + ament_export_dependencies(ament_index_cpp) set( BEHAVIOR_TREE_LIB_DESTINATION lib ) @@ -246,11 +250,20 @@ if(ament_cmake_FOUND) ament_export_include_directories(include) ament_export_libraries(${BEHAVIOR_TREE_LIBRARY}) + ament_export_targets(${BEHAVIOR_TREE_LIBRARY}Targets) ament_package() elseif(catkin_FOUND) + + target_include_directories(${BEHAVIOR_TREE_LIBRARY} PRIVATE + ${catkin_INCLUDE_DIRS} ) + + target_link_libraries(${BEHAVIOR_TREE_LIBRARY} PRIVATE + ${catkin_LIBRARIES} ) + 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 ) @@ -262,63 +275,57 @@ message( STATUS "BEHAVIOR_TREE_LIB_DESTINATION: ${BEHAVIOR_TREE_LIB_DESTINATIO message( STATUS "BEHAVIOR_TREE_BIN_DESTINATION: ${BEHAVIOR_TREE_BIN_DESTINATION} " ) message( STATUS "BUILD_UNIT_TESTS: ${BUILD_UNIT_TESTS} " ) - -###################################################### -# Samples -if (BUILD_SAMPLES) - add_subdirectory(sample_nodes) -endif() +add_subdirectory(sample_nodes) ###################################################### # Test -if (BUILD_UNIT_TESTS AND BUILD_SAMPLES) +if (BUILD_UNIT_TESTS) add_subdirectory(tests) endif() ###################################################### # INSTALL -INSTALL(TARGETS ${BEHAVIOR_TREE_LIBRARY} - EXPORT ${PROJECT_NAME}Targets - 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 ${PROJECT_NAME}Targets - FILE "${PROJECT_NAME}Targets.cmake" - DESTINATION "${BEHAVIOR_TREE_LIB_DESTINATION}/cmake/${PROJECT_NAME}" - NAMESPACE BT:: +if(catkin_FOUND) + INSTALL(TARGETS ${BEHAVIOR_TREE_LIBRARY} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} + ) +else() + INSTALL(TARGETS ${BEHAVIOR_TREE_LIBRARY} + EXPORT ${PROJECT_NAME}Targets + ARCHIVE DESTINATION ${BEHAVIOR_TREE_LIB_DESTINATION} + LIBRARY DESTINATION ${BEHAVIOR_TREE_LIB_DESTINATION} + RUNTIME DESTINATION ${BEHAVIOR_TREE_BIN_DESTINATION} ) -export(PACKAGE ${PROJECT_NAME}) + install(EXPORT ${PROJECT_NAME}Targets + FILE "${PROJECT_NAME}Targets.cmake" + DESTINATION "${BEHAVIOR_TREE_LIB_DESTINATION}/cmake/${PROJECT_NAME}" + NAMESPACE BT:: + ) -include(CMakePackageConfigHelpers) + install(FILES + "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Config.cmake" + DESTINATION "${BEHAVIOR_TREE_LIB_DESTINATION}/cmake/${PROJECT_NAME}" + ) -configure_package_config_file( - "${CMAKE_CURRENT_SOURCE_DIR}/cmake/Config.cmake.in" - "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Config.cmake" - INSTALL_DESTINATION "${BEHAVIOR_TREE_LIB_DESTINATION}/cmake/${PROJECT_NAME}" -) + export(PACKAGE ${PROJECT_NAME}) -# This requires to declare to project version in the project() macro + include(CMakePackageConfigHelpers) -#write_basic_package_version_file( -# "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}ConfigVersion.cmake" -# VERSION ${PROJECT_VERSION} -# COMPATIBILITY AnyNewerVersion -#) + configure_package_config_file( + "${CMAKE_CURRENT_SOURCE_DIR}/cmake/Config.cmake.in" + "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Config.cmake" + INSTALL_DESTINATION "${BEHAVIOR_TREE_LIB_DESTINATION}/cmake/${PROJECT_NAME}" + ) +endif() -install( - FILES - "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Config.cmake" - # "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}ConfigVersion.cmake" - DESTINATION "${BEHAVIOR_TREE_LIB_DESTINATION}/cmake/${PROJECT_NAME}" -) ###################################################### # EXAMPLES and TOOLS @@ -326,6 +333,6 @@ if(BUILD_TOOLS) add_subdirectory(tools) endif() -if(BUILD_EXAMPLES AND BUILD_SAMPLES) +if(BUILD_EXAMPLES) add_subdirectory(examples) endif() diff --git a/README.md b/README.md index 6d0063a35..4a1263264 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,5 @@ ![License MIT](https://img.shields.io/github/license/BehaviorTree/BehaviorTree.CPP?color=blue) -![Version](https://img.shields.io/badge/version-3.7-blue.svg) +![Version](https://img.shields.io/badge/version-3.8-blue.svg) [![cmake](https://github.com/BehaviorTree/BehaviorTree.CPP/actions/workflows/cmake.yml/badge.svg)](https://github.com/BehaviorTree/BehaviorTree.CPP/actions/workflows/cmake.yml) [![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) diff --git a/examples/t01_build_your_first_tree.cpp b/examples/t01_build_your_first_tree.cpp index 49d270ce8..de0cd3acb 100644 --- a/examples/t01_build_your_first_tree.cpp +++ b/examples/t01_build_your_first_tree.cpp @@ -59,14 +59,12 @@ int main() // Registering a SimpleActionNode using a function pointer. // you may also use C++11 lambdas instead of std::bind - factory.registerSimpleCondition("CheckBattery", std::bind(CheckBattery)); + factory.registerSimpleCondition("CheckBattery", [&](TreeNode&) { return 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)); + factory.registerSimpleAction("OpenGripper", [&](TreeNode&){ return gripper.open(); } ); + factory.registerSimpleAction("CloseGripper", [&](TreeNode&){ return gripper.close(); } ); #else // Load dynamically a plugin and register the TreeNodes it contains @@ -83,7 +81,7 @@ int main() // 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(); + tree.tickRootWhileRunning(); return 0; } diff --git a/examples/t02_basic_ports.cpp b/examples/t02_basic_ports.cpp index df0d14b9d..c68fc214b 100644 --- a/examples/t02_basic_ports.cpp +++ b/examples/t02_basic_ports.cpp @@ -95,8 +95,7 @@ int main() */ auto tree = factory.createTreeFromText(xml_text); - - tree.tickRoot(); + tree.tickRootWhileRunning(); /* Expected output: * diff --git a/examples/t03_generic_ports.cpp b/examples/t03_generic_ports.cpp index 848b1d80e..bbf93f19a 100644 --- a/examples/t03_generic_ports.cpp +++ b/examples/t03_generic_ports.cpp @@ -126,7 +126,7 @@ int main() factory.registerNodeType("PrintTarget"); auto tree = factory.createTreeFromText(xml_text); - tree.tickRoot(); + tree.tickRootWhileRunning(); /* Expected output: * diff --git a/examples/t04_reactive_sequence.cpp b/examples/t04_reactive_sequence.cpp index 05b819b3b..cd50eedc6 100644 --- a/examples/t04_reactive_sequence.cpp +++ b/examples/t04_reactive_sequence.cpp @@ -80,56 +80,68 @@ int main() auto tree = factory.createTreeFromText(xml_text); - NodeStatus status; - - std::cout << "\n--- 1st executeTick() ---" << std::endl; - status = tree.tickRoot(); - Assert(status == NodeStatus::RUNNING); - - tree.sleep(milliseconds(150)); - std::cout << "\n--- 2nd executeTick() ---" << std::endl; - status = tree.tickRoot(); - Assert(status == NodeStatus::RUNNING); - - tree.sleep(milliseconds(150)); - std::cout << "\n--- 3rd executeTick() ---" << std::endl; - status = tree.tickRoot(); - Assert(status == NodeStatus::SUCCESS); - - std::cout << std::endl; + // Here, instead of tree.tickRootWhileRunning(), + // we prefer our own loop. + std::cout << "--- ticking\n"; + NodeStatus status = tree.tickRoot(); + std::cout << "--- status: " << toStr(status) << "\n\n"; + + while(status == NodeStatus::RUNNING) + { + // Sleep to avoid busy loops. + // do NOT use other sleep functions! + // Small sleep time is OK, here we use a large one only to + // have less messages on the console. + tree.sleep(std::chrono::milliseconds(100)); + + std::cout << "--- ticking\n"; + status = tree.tickRoot(); + std::cout << "--- status: " << toStr(status) << "\n\n"; + } } return 0; } /* - Expected output: +* Expected output: ------------ BUILDING A NEW TREE ------------ - ---- 1st executeTick() --- +--- ticking [ Battery: OK ] -Robot says: "mission started..." +Robot says: mission started... +--- status: RUNNING + [ MoveBase: STARTED ]. goal: x=1 y=2.0 theta=3.00 +--- ticking +--- status: RUNNING + +--- ticking +--- status: RUNNING ---- 2nd executeTick() --- [ MoveBase: FINISHED ] +--- ticking +Robot says: mission completed! +--- status: SUCCESS ---- 3rd executeTick() --- -Robot says: "mission completed!" ------------ BUILDING A NEW TREE ------------ - ---- 1st executeTick() --- +--- ticking [ Battery: OK ] -Robot says: "mission started..." -[ MoveBase: STARTED ]. goal: x=1 y=2.0 theta=3.00 +Robot says: mission started... +--- status: RUNNING ---- 2nd executeTick() --- +[ MoveBase: STARTED ]. goal: x=1 y=2.0 theta=3.00 +--- ticking [ Battery: OK ] -[ MoveBase: FINISHED ] +--- status: RUNNING ---- 3rd executeTick() --- +--- ticking [ Battery: OK ] -Robot says: "mission completed!" +--- status: RUNNING +[ MoveBase: FINISHED ] +--- ticking +[ Battery: OK ] +Robot says: mission completed! +--- status: SUCCESS */ diff --git a/examples/t05_crossdoor.cpp b/examples/t05_crossdoor.cpp index dbf7a5f01..f4938b01b 100644 --- a/examples/t05_crossdoor.cpp +++ b/examples/t05_crossdoor.cpp @@ -86,25 +86,12 @@ int main(int argc, char** argv) const bool LOOP = (argc == 2 && strcmp(argv[1], "loop") == 0); - using std::chrono::milliseconds; - do + NodeStatus status = tree.tickRoot(); + while(LOOP || status == NodeStatus::RUNNING) { - NodeStatus status = NodeStatus::RUNNING; - // Keep on ticking until you get either a SUCCESS or FAILURE state - while (status == NodeStatus::RUNNING) - { - status = tree.tickRoot(); - // IMPORTANT: you must always add some sleep if you call tickRoot() - // in a loop, to avoid using 100% of your CPU (busy loop). - // The method Tree::sleep() is recommended, because it can be - // interrupted by an internal event inside the tree. - tree.sleep(milliseconds(10)); - } - if (LOOP) - { - std::this_thread::sleep_for(milliseconds(1000)); - } - } while (LOOP); + tree.sleep(std::chrono::milliseconds(10)); + status = tree.tickRoot(); + } return 0; } diff --git a/examples/t06_subtree_port_remapping.cpp b/examples/t06_subtree_port_remapping.cpp index 760473ec6..eaf885a91 100644 --- a/examples/t06_subtree_port_remapping.cpp +++ b/examples/t06_subtree_port_remapping.cpp @@ -62,15 +62,7 @@ int main() 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(); - // IMPORTANT: add sleep to avoid busy loops. - // You should use Tree::sleep(). Don't be afraid to run this at 1 KHz. - tree.sleep(std::chrono::milliseconds(1)); - } + tree.tickRootWhileRunning(); // let's visualize some information about the current state of the blackboards. std::cout << "--------------" << std::endl; diff --git a/examples/t07_load_multiple_xml.cpp b/examples/t07_load_multiple_xml.cpp index 2028f5850..21b8ae0c6 100644 --- a/examples/t07_load_multiple_xml.cpp +++ b/examples/t07_load_multiple_xml.cpp @@ -58,12 +58,12 @@ int main() // You can create the MainTree and the subtrees will be added automatically. std::cout << "----- MainTree tick ----" << std::endl; auto main_tree = factory.createTree("MainTree"); - main_tree.tickRoot(); + main_tree.tickRootWhileRunning(); // ... or you can create only one of the subtree std::cout << "----- SubA tick ----" << std::endl; auto subA_tree = factory.createTree("SubA"); - subA_tree.tickRoot(); + subA_tree.tickRootWhileRunning(); return 0; } diff --git a/examples/t08_additional_node_args.cpp b/examples/t08_additional_node_args.cpp index 7d3d5e4af..a5a71c77e 100644 --- a/examples/t08_additional_node_args.cpp +++ b/examples/t08_additional_node_args.cpp @@ -111,7 +111,7 @@ int main() } } - tree.tickRoot(); + tree.tickRootWhileRunning(); /* Expected output: diff --git a/examples/t09_async_actions_coroutines.cpp b/examples/t09_async_actions_coroutines.cpp index a21c1c53f..4c1404f80 100644 --- a/examples/t09_async_actions_coroutines.cpp +++ b/examples/t09_async_actions_coroutines.cpp @@ -114,11 +114,8 @@ int main() auto tree = factory.createTreeFromText(xml_text); //--------------------------------------- - // keep executing tick until it returns either SUCCESS or FAILURE - while (tree.tickRoot() == NodeStatus::RUNNING) - { - tree.sleep(std::chrono::milliseconds(10)); - } + tree.tickRootWhileRunning(); + return 0; } diff --git a/include/behaviortree_cpp_v3/actions/set_blackboard_node.h b/include/behaviortree_cpp_v3/actions/set_blackboard_node.h index bbdc080b8..8c30b381b 100644 --- a/include/behaviortree_cpp_v3/actions/set_blackboard_node.h +++ b/include/behaviortree_cpp_v3/actions/set_blackboard_node.h @@ -26,6 +26,12 @@ namespace BT * * * Will store the string "42" in the entry with key "the_answer". + * + * Alternatively, you can use it to copy one port inside another port: + * + * + * + * This will copy the type and content of {src_port} into {dst_port} */ class SetBlackboard : public SyncActionNode { @@ -38,26 +44,45 @@ class SetBlackboard : public SyncActionNode static PortsList providedPorts() { - return {InputPort("value", "Value represented as a string. convertFromString must be " - "implemented."), + return {InputPort("value", "Value to be written int othe output_key"), BidirectionalPort("output_key", "Name of the blackboard entry where the " - "value " - "should be written")}; + "value should be written")}; } private: virtual BT::NodeStatus tick() override { - std::string key, value; - if (!getInput("output_key", key)) + std::string output_key; + if (!getInput("output_key", output_key)) { throw RuntimeError("missing port [output_key]"); } - if (!getInput("value", value)) + + const std::string value_str = config().input_ports.at("value"); + + if(isBlackboardPointer(value_str)) { - throw RuntimeError("missing port [value]"); + StringView stripped_key = stripBlackboardPointer(value_str); + const auto input_key = std::string(stripped_key); + std::shared_ptr src_entry = config().blackboard->getEntry(input_key); + std::shared_ptr dst_entry = config().blackboard->getEntry(output_key); + + if(!src_entry) + { + throw RuntimeError("Can't find the port referred by [value]"); + } + if(!dst_entry) + { + config().blackboard->createEntry(output_key, src_entry->port_info); + dst_entry = config().blackboard->getEntry(output_key); + } + dst_entry->value = src_entry->value; + } + else + { + config().blackboard->set(output_key, value_str); } - setOutput("output_key", value); + return NodeStatus::SUCCESS; } }; diff --git a/include/behaviortree_cpp_v3/basic_types.h b/include/behaviortree_cpp_v3/basic_types.h index 9572d2497..2ec051cf1 100644 --- a/include/behaviortree_cpp_v3/basic_types.h +++ b/include/behaviortree_cpp_v3/basic_types.h @@ -216,7 +216,11 @@ using Optional = nonstd::expected; * */ using Result = Optional; -const std::unordered_set ReservedPortNames = {"ID", "name", "_description"}; +inline bool IsReservedPortname(StringView name) +{ + return name == "ID" || name == "name" || name == "_description"; +} + class PortInfo { @@ -253,6 +257,11 @@ class PortInfo const std::string& defaultValue() const; + bool isStronglyTyped() const + { + return _info != nullptr; + } + private: PortDirection _type; const std::type_info* _info; @@ -266,7 +275,7 @@ std::pair CreatePort(PortDirection direction, StringView StringView description = {}) { auto sname = static_cast(name); - if (ReservedPortNames.count(sname) != 0) + if (IsReservedPortname(sname)) { throw std::runtime_error("A port can not use a reserved name. See ReservedPortNames"); } diff --git a/include/behaviortree_cpp_v3/blackboard.h b/include/behaviortree_cpp_v3/blackboard.h index 29b18542c..12cd71502 100644 --- a/include/behaviortree_cpp_v3/blackboard.h +++ b/include/behaviortree_cpp_v3/blackboard.h @@ -40,42 +40,21 @@ class Blackboard virtual ~Blackboard() = default; + void enableAutoRemapping(bool remapping); + /** * @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 lock(mutex_); - // search first if this port was remapped - 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); - } + const Any* getAny(const std::string& key) const; Any* getAny(const std::string& key) { - std::unique_lock lock(mutex_); - // search first if this port was remapped - 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); + // "Avoid Duplication in const and Non-const Member Function," + // on p. 23, in Item 3 "Use const whenever possible," in Effective C++, 3d ed + return const_cast(static_cast(*this).getAny(key)); } /** Return true if the entry with the given key was found. @@ -116,65 +95,62 @@ class Blackboard std::unique_lock lock_entry(entry_mutex_); std::unique_lock lock(mutex_); - // search first if this port was remapped. - // Change the parent_bb_ in that case - auto remapping_it = internal_to_external_.find(key); - if (remapping_it != internal_to_external_.end()) + // check local storage + auto it = storage_.find(key); + std::shared_ptr entry; + if (it != storage_.end()) { - const auto& remapped_key = remapping_it->second; - if (auto parent = parent_bb_.lock()) + entry = it->second; + } + else + { + Any new_value(value); + std::shared_ptr entry; + lock.unlock(); + if(std::is_constructible::value) { - parent->set(remapped_key, value); - return; + entry = createEntryImpl(key, PortInfo(PortDirection::INOUT)); + } + else { + entry = createEntryImpl(key, PortInfo(PortDirection::INOUT, new_value.type(), {})); } + entry->value = new_value; + return; } - // check local storage - auto it = storage_.find(key); - if (it != storage_.end()) - { - const PortInfo& port_info = it->second.port_info; - auto& previous_any = it->second.value; - const auto previous_type = port_info.type(); + const PortInfo& port_info = entry->port_info; + auto& previous_any = entry->value; + const auto previous_type = port_info.type(); - Any new_value(value); + Any new_value(value); - if (previous_type && *previous_type != typeid(T) && - *previous_type != new_value.type()) + if (previous_type && *previous_type != typeid(T) && + *previous_type != new_value.type()) + { + bool mismatching = true; + if (std::is_constructible::value) { - bool mismatching = true; - if (std::is_constructible::value) + Any any_from_string = port_info.parseString(value); + if (any_from_string.empty() == false) { - Any any_from_string = port_info.parseString(value); - if (any_from_string.empty() == false) - { - mismatching = false; - new_value = std::move(any_from_string); - } + mismatching = false; + new_value = std::move(any_from_string); } + } - if (mismatching) - { - debugMessage(); + if (mismatching) + { + debugMessage(); - throw LogicError("Blackboard::set() failed: once declared, the type of a port " - "shall not change. " - "Declared type [", - BT::demangle(previous_type), "] != current type [", - BT::demangle(typeid(T)), "]"); - } + throw LogicError("Blackboard::set() failed: once declared, the type of a port " + "shall not change. Declared type [", + BT::demangle(previous_type), "] != current type [", + BT::demangle(typeid(T)), "]"); } - previous_any = std::move(new_value); } - else - { // create for the first time without any info - storage_.emplace(key, Entry(Any(value), PortInfo())); - } - return; + previous_any = std::move(new_value); } - void setPortInfo(std::string key, const PortInfo& info); - const PortInfo* portInfo(const std::string& key); void addSubtreeRemapping(StringView internal, StringView external); @@ -197,7 +173,11 @@ class Blackboard return entry_mutex_; } -private: + void createEntry(const std::string& key, const PortInfo& info) + { + createEntryImpl(key, info); + } + struct Entry { Any value; @@ -207,15 +187,27 @@ class Blackboard {} Entry(Any&& other_any, const PortInfo& info) : - value(std::move(other_any)), port_info(info) + value(std::move(other_any)), port_info(info) {} }; + std::shared_ptr getEntry(const std::string& key) const + { + auto it = storage_.find(key); + return it == storage_.end() ? std::shared_ptr() : it->second; + } + +private: + + std::shared_ptr createEntryImpl(const std::string& key, const PortInfo& info); + mutable std::mutex mutex_; mutable std::mutex entry_mutex_; - std::unordered_map storage_; + std::unordered_map> storage_; std::weak_ptr parent_bb_; std::unordered_map internal_to_external_; + + bool autoremapping_ = false; }; } // namespace BT diff --git a/include/behaviortree_cpp_v3/bt_factory.h b/include/behaviortree_cpp_v3/bt_factory.h index 4191d6329..dcad95309 100644 --- a/include/behaviortree_cpp_v3/bt_factory.h +++ b/include/behaviortree_cpp_v3/bt_factory.h @@ -81,40 +81,38 @@ inline TreeNodeManifest CreateManifest(const std::string& ID, return {getType(), ID, portlist, {}}; } -constexpr const char* PLUGIN_SYMBOL = "BT_RegisterNodesFromPlugin"; +#ifdef BT_PLUGIN_EXPORT -#ifndef BT_PLUGIN_EXPORT +#if defined(_WIN32) + #define BTCPP_EXPORT extern "C" __declspec(dllexport) +#else + // Unix-like OSes + #define BTCPP_EXPORT extern "C" __attribute__ ((visibility ("default"))) +#endif +#else + #define BTCPP_EXPORT static +#endif /* Use this macro to automatically register one or more custom Nodes -into a factory. For instance: - -BT_REGISTER_NODES(factory) -{ - factory.registerNodeType("MoveBase"); -} +* into a factory. For instance: +* +* BT_REGISTER_NODES(factory) +* { +* factory.registerNodeType("MoveBase"); +* } +* +* IMPORTANT: this function MUST be declared in a cpp file, NOT a header file. +* In your cake, you must add the definition [BT_PLUGIN_EXPORT] with: +* +* target_compile_definitions(my_plugin_target PRIVATE BT_PLUGIN_EXPORT ) -IMPORTANT: this must funtion MUST be declared in a cpp file, NOT a header file. -See examples for more information about configuring CMake correctly +* See examples in sample_nodes directory. */ -#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 + BTCPP_EXPORT void BT_RegisterNodesFromPlugin(BT::BehaviorTreeFactory& factory) -#define BT_REGISTER_NODES(factory) \ - extern "C" void __declspec(dllexport) \ - BT_RegisterNodesFromPlugin(BT::BehaviorTreeFactory& factory) -#endif - -#endif +constexpr const char* PLUGIN_SYMBOL = "BT_RegisterNodesFromPlugin"; /** * @brief Struct used to store a tree. @@ -185,6 +183,30 @@ class Tree return nodes.empty() ? nullptr : nodes.front().get(); } + /** + * @brief tickRootWhileRunning imply execute tickRoot in a loop + * as long as the status is RUNNING. + * + * @param sleep_time maximum sleep time between consecutive loops. + * + * @return should return only SUCCESS or FAILURE. + */ + NodeStatus tickRootWhileRunning(std::chrono::milliseconds sleep_time = std::chrono::milliseconds(10)) + { + NodeStatus status = tickRoot(); + + while (status == NodeStatus::RUNNING) + { + this->sleep(sleep_time); + status = tickRoot(); + } + return status; + } + + /** + * @brief tickRoot send the tick signal to the root node. + * It will propagate through the entire tree. + */ NodeStatus tickRoot() { if (!wake_up_) diff --git a/include/behaviortree_cpp_v3/control_node.h b/include/behaviortree_cpp_v3/control_node.h index 3f8de1dde..306a05cba 100644 --- a/include/behaviortree_cpp_v3/control_node.h +++ b/include/behaviortree_cpp_v3/control_node.h @@ -42,6 +42,7 @@ class ControlNode : public TreeNode virtual void halt() override; + /// same as resetChildren() void haltChildren(); [[deprecated("deprecated: please use explicitly haltChildren() or haltChild(i)")]] void @@ -53,5 +54,9 @@ class ControlNode : public TreeNode { return NodeType::CONTROL; } + + /// Set the status of all children to IDLE. + /// also send a halt() signal to all RUNNING children + void resetChildren(); }; } // namespace BT diff --git a/include/behaviortree_cpp_v3/controls/reactive_fallback.h b/include/behaviortree_cpp_v3/controls/reactive_fallback.h index af9253963..bb4fc6734 100644 --- a/include/behaviortree_cpp_v3/controls/reactive_fallback.h +++ b/include/behaviortree_cpp_v3/controls/reactive_fallback.h @@ -36,8 +36,19 @@ class ReactiveFallback : public ControlNode ReactiveFallback(const std::string& name) : ControlNode(name, {}) {} + /** A ReactiveFallback is not supposed to have more than a single + * anychronous node; if it does an exception is thrown. + * You can disabled that check, if you know what you are doing. + */ + static void EnableException(bool enable); + private: - virtual BT::NodeStatus tick() override; + BT::NodeStatus tick() override; + + void halt() override; + + int running_child_ = -1; + static bool throw_if_multiple_running; }; } // namespace BT diff --git a/include/behaviortree_cpp_v3/controls/reactive_sequence.h b/include/behaviortree_cpp_v3/controls/reactive_sequence.h index 3f05b1dfd..2f47d1dac 100644 --- a/include/behaviortree_cpp_v3/controls/reactive_sequence.h +++ b/include/behaviortree_cpp_v3/controls/reactive_sequence.h @@ -36,8 +36,20 @@ class ReactiveSequence : public ControlNode ReactiveSequence(const std::string& name) : ControlNode(name, {}) {} + /** A ReactiveSequence is not supposed to have more than a single + * anychronous node; if it does an exception is thrown. + * You can disabled that check, if you know what you are doing. + */ + static void EnableException(bool enable); + private: - virtual BT::NodeStatus tick() override; + BT::NodeStatus tick() override; + + void halt() override; + + int running_child_ = -1; + + static bool throw_if_multiple_running; }; } // namespace BT diff --git a/include/behaviortree_cpp_v3/controls/switch_node.h b/include/behaviortree_cpp_v3/controls/switch_node.h index 30b341594..596087fc7 100644 --- a/include/behaviortree_cpp_v3/controls/switch_node.h +++ b/include/behaviortree_cpp_v3/controls/switch_node.h @@ -119,7 +119,7 @@ inline NodeStatus SwitchNode::tick() } else { - haltChildren(); + resetChildren(); running_child_ = -1; } return ret; diff --git a/include/behaviortree_cpp_v3/decorator_node.h b/include/behaviortree_cpp_v3/decorator_node.h index 546d62d7b..3b40dabc9 100644 --- a/include/behaviortree_cpp_v3/decorator_node.h +++ b/include/behaviortree_cpp_v3/decorator_node.h @@ -24,7 +24,7 @@ class DecoratorNode : public TreeNode /// The method used to interrupt the execution of this node virtual void halt() override; - /// Halt() the child node + /// Same as resetChild() void haltChild(); virtual NodeType type() const override @@ -33,6 +33,10 @@ class DecoratorNode : public TreeNode } NodeStatus executeTick() override; + + /// Set the status of the child to IDLE. + /// also send a halt() signal to a RUNNING child + void resetChild(); }; /** diff --git a/include/behaviortree_cpp_v3/decorators/force_failure_node.h b/include/behaviortree_cpp_v3/decorators/force_failure_node.h index 4eaebcfe3..d8e09d5d6 100644 --- a/include/behaviortree_cpp_v3/decorators/force_failure_node.h +++ b/include/behaviortree_cpp_v3/decorators/force_failure_node.h @@ -37,23 +37,15 @@ inline NodeStatus ForceFailureNode::tick() { setStatus(NodeStatus::RUNNING); - const NodeStatus child_state = child_node_->executeTick(); + const NodeStatus child_status = child_node_->executeTick(); - switch (child_state) + if(StatusCompleted(child_status)) { - case NodeStatus::FAILURE: - case NodeStatus::SUCCESS: { - return NodeStatus::FAILURE; - } - - case NodeStatus::RUNNING: { - return NodeStatus::RUNNING; - } - - default: { - // TODO throw? - } + resetChild(); + return NodeStatus::FAILURE; } - return status(); + + // RUNNING + return child_status; } } // namespace BT diff --git a/include/behaviortree_cpp_v3/decorators/force_success_node.h b/include/behaviortree_cpp_v3/decorators/force_success_node.h index 9e0c89ae7..307427f09 100644 --- a/include/behaviortree_cpp_v3/decorators/force_success_node.h +++ b/include/behaviortree_cpp_v3/decorators/force_success_node.h @@ -37,23 +37,15 @@ inline NodeStatus ForceSuccessNode::tick() { setStatus(NodeStatus::RUNNING); - const NodeStatus child_state = child_node_->executeTick(); + const NodeStatus child_status = child_node_->executeTick(); - switch (child_state) + if(StatusCompleted(child_status)) { - case NodeStatus::FAILURE: - case NodeStatus::SUCCESS: { - return NodeStatus::SUCCESS; - } - - case NodeStatus::RUNNING: { - return NodeStatus::RUNNING; - } - - default: { - // TODO throw? - } + resetChild(); + return NodeStatus::SUCCESS; } - return status(); + + // RUNNING + return child_status; } } // namespace BT 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 index 27f9bb8cd..44d79585e 100644 --- a/include/behaviortree_cpp_v3/decorators/keep_running_until_failure_node.h +++ b/include/behaviortree_cpp_v3/decorators/keep_running_until_failure_node.h @@ -43,9 +43,13 @@ inline NodeStatus KeepRunningUntilFailureNode::tick() switch (child_state) { case NodeStatus::FAILURE: { + resetChild(); return NodeStatus::FAILURE; } - case NodeStatus::SUCCESS: + case NodeStatus::SUCCESS: { + resetChild(); + return NodeStatus::RUNNING; + } case NodeStatus::RUNNING: { return NodeStatus::RUNNING; } diff --git a/include/behaviortree_cpp_v3/decorators/timeout_node.h b/include/behaviortree_cpp_v3/decorators/timeout_node.h index dd1bd67eb..188e6012c 100644 --- a/include/behaviortree_cpp_v3/decorators/timeout_node.h +++ b/include/behaviortree_cpp_v3/decorators/timeout_node.h @@ -77,8 +77,15 @@ class TimeoutNode : public DecoratorNode if (msec_ > 0) { timer_id_ = timer_.add(std::chrono::milliseconds(msec_), [this](bool aborted) { + // Return immediately if the timer was aborted. + // This function could be invoked during destruction of this object and + // we don't want to access member variables if not needed. + if (aborted) + { + return; + } std::unique_lock lk(timeout_mutex_); - if (!aborted && child()->status() == NodeStatus::RUNNING) + if (child()->status() == NodeStatus::RUNNING) { child_halted_ = true; haltChild(); @@ -97,13 +104,14 @@ class TimeoutNode : public DecoratorNode } else { - auto child_status = child()->executeTick(); - if (child_status != NodeStatus::RUNNING) + const NodeStatus child_status = child()->executeTick(); + if(StatusCompleted(child_status)) { timeout_started_ = false; timeout_mutex_.unlock(); timer_.cancel(timer_id_); timeout_mutex_.lock(); + resetChild(); } return child_status; } diff --git a/include/behaviortree_cpp_v3/flatbuffers/base.h b/include/behaviortree_cpp_v3/flatbuffers/base.h index 54a51aacb..abe0b95e8 100644 --- a/include/behaviortree_cpp_v3/flatbuffers/base.h +++ b/include/behaviortree_cpp_v3/flatbuffers/base.h @@ -237,12 +237,17 @@ namespace flatbuffers { } #define FLATBUFFERS_HAS_STRING_VIEW 1 // Check for absl::string_view - #elif __has_include("absl/strings/string_view.h") - #include "absl/strings/string_view.h" - namespace flatbuffers { - typedef absl::string_view string_view; - } - #define FLATBUFFERS_HAS_STRING_VIEW 1 + #elif __has_include("absl/strings/string_view.h") && \ + __has_include("absl/base/config.h") && \ + (__cplusplus >= 201411) + #include "absl/base/config.h" + #if !defined(ABSL_USES_STD_STRING_VIEW) + #include "absl/strings/string_view.h" + namespace flatbuffers { + typedef absl::string_view string_view; + } + #define FLATBUFFERS_HAS_STRING_VIEW 1 + #endif #endif #endif // __has_include #endif // !FLATBUFFERS_HAS_STRING_VIEW diff --git a/include/behaviortree_cpp_v3/flatbuffers/stl_emulation.h b/include/behaviortree_cpp_v3/flatbuffers/stl_emulation.h index 77e0f6610..ec10dc171 100644 --- a/include/behaviortree_cpp_v3/flatbuffers/stl_emulation.h +++ b/include/behaviortree_cpp_v3/flatbuffers/stl_emulation.h @@ -625,7 +625,7 @@ class span FLATBUFFERS_FINAL_CLASS { private: // This is a naive implementation with 'count_' member even if (Extent != dynamic_extent). pointer const data_; - const size_type count_; + size_type count_; }; #if !defined(FLATBUFFERS_SPAN_MINIMAL) diff --git a/include/behaviortree_cpp_v3/tree_node.h b/include/behaviortree_cpp_v3/tree_node.h index e057760b2..d6d203125 100644 --- a/include/behaviortree_cpp_v3/tree_node.h +++ b/include/behaviortree_cpp_v3/tree_node.h @@ -200,6 +200,9 @@ class TreeNode void setStatus(NodeStatus new_status); + /// Equivalent to setStatus(NodeStatus::IDLE) + void resetStatus(); + private: const std::string name_; @@ -222,9 +225,6 @@ class TreeNode PostTickOverrideCallback post_condition_callback_; std::shared_ptr wake_up_; - - /// Set the status to IDLE - void resetStatus(); }; //------------------------------------------------------- @@ -258,24 +258,30 @@ inline Result TreeNode::getInput(const std::string& key, T& destination) const std::unique_lock entry_lock(config_.blackboard->entryMutex()); const Any* val = config_.blackboard->getAny(static_cast(remapped_key)); - if (val && val->empty() == false) + + if(!val) { - if (std::is_same::value == false && - val->type() == typeid(std::string)) - { - destination = convertFromString(val->cast()); - } - else - { - destination = val->cast(); - } - return {}; + return nonstd::make_unexpected(StrCat("getInput() failed because it was unable to " + "find the port [", key, + "] remapped to BB [", remapped_key, "]")); } - return nonstd::make_unexpected(StrCat("getInput() failed because it was unable to " - "find the " - "key [", - key, "] remapped to [", remapped_key, "]")); + if(val->empty()) + { + return nonstd::make_unexpected(StrCat("getInput() failed because the port [", key, + "] remapped to BB [", remapped_key, "] was found," + "but its content was not initialized correctly")); + } + + if (!std::is_same::value && val->type() == typeid(std::string)) + { + destination = convertFromString(val->cast()); + } + else + { + destination = val->cast(); + } + return {}; } catch (std::exception& err) { diff --git a/include/behaviortree_cpp_v3/utils/shared_library.h b/include/behaviortree_cpp_v3/utils/shared_library.h index 1ef19d8cd..c807b6262 100644 --- a/include/behaviortree_cpp_v3/utils/shared_library.h +++ b/include/behaviortree_cpp_v3/utils/shared_library.h @@ -51,13 +51,12 @@ class SharedLibrary 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_GLOBAL = 1, - 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 @@ -65,21 +64,21 @@ class SharedLibrary /// 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(). + SHLIB_LOCAL = 2 }; - SharedLibrary(); /// Creates a SharedLibrary object. + SharedLibrary(); - 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. + SharedLibrary(const std::string& path, int flags = 0); - virtual ~SharedLibrary() = default; /// Destroys the SharedLibrary. The actual library /// remains loaded. + virtual ~SharedLibrary() = default; - 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. @@ -87,45 +86,46 @@ class SharedLibrary /// a library has already been loaded. /// Throws a LibraryLoadException if the library /// cannot be loaded. + void load(const std::string& path, int flags = 0); - void unload(); /// Unloads a shared library. + void unload(); - bool isLoaded() const; /// Returns true iff a library has been loaded. + bool isLoaded() const; - bool hasSymbol(const std::string& name); /// Returns true iff the loaded library contains /// a symbol with the given name. + bool hasSymbol(const std::string& 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. + void* getSymbol(const std::string& name); - const std::string& getPath() const; /// Returns the path of the library, as /// specified in a call to load() or the /// constructor. + const std::string& getPath() const; - 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 prefix(); - 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 suffix(); - 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() + static std::string getOSName(const std::string& name); private: SharedLibrary(const SharedLibrary&); @@ -134,7 +134,7 @@ class SharedLibrary void* findSymbol(const std::string& name); std::string _path; - void* _handle; + void* _handle = nullptr; std::mutex _mutex; }; diff --git a/package.xml b/package.xml index 9588ea247..d186d59cb 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ behaviortree_cpp_v3 - 3.7.0 + 3.8.7 This package provides the Behavior Trees core library. @@ -22,7 +22,8 @@ rclcpp ament_index_cpp - boost + libboost-coroutine-dev + libboost-coroutine libzmq3-dev libncurses-dev diff --git a/src/blackboard.cpp b/src/blackboard.cpp index e5a46c091..12b1bc266 100644 --- a/src/blackboard.cpp +++ b/src/blackboard.cpp @@ -2,59 +2,46 @@ namespace BT { -void Blackboard::setPortInfo(std::string key, const PortInfo& info) +void Blackboard::enableAutoRemapping(bool remapping) +{ + autoremapping_ = remapping; +} + +const Any *Blackboard::getAny(const std::string &key) const { std::unique_lock lock(mutex_); + auto it = storage_.find(key); - if (auto parent = parent_bb_.lock()) + if(it == storage_.end()) { + // Try with autoremapping. This should work recursively auto remapping_it = internal_to_external_.find(key); if (remapping_it != internal_to_external_.end()) { - parent->setPortInfo(remapping_it->second, info); - return; + const auto& remapped_key = remapping_it->second; + if (auto parent = parent_bb_.lock()) + { + return parent->getAny(remapped_key); + } } - } - auto it = storage_.find(key); - if (it == storage_.end()) - { - storage_.emplace(key, Entry(info)); - } - else - { - auto old_type = it->second.port_info.type(); - if (old_type && old_type != info.type()) + else if(autoremapping_) { - 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()), "]"); + if(auto parent = parent_bb_.lock()) { + return parent->getAny(key); + } } + return nullptr; } + return &(it->second->value); } const PortInfo* Blackboard::portInfo(const std::string& key) { std::unique_lock 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->portInfo(remapping_it->second); - } - } - auto it = storage_.find(key); - if (it == storage_.end()) - { - return nullptr; - } - return &(it->second.port_info); + return (it == storage_.end()) ? nullptr : &(it->second->port_info); } void Blackboard::addSubtreeRemapping(StringView internal, StringView external) @@ -65,26 +52,29 @@ void Blackboard::addSubtreeRemapping(StringView internal, StringView external) void Blackboard::debugMessage() const { - for (const auto& entry_it : storage_) + for (const auto& it: storage_) { - auto port_type = entry_it.second.port_info.type(); + const auto& key = it.first; + const auto& entry = it.second; + + auto port_type = entry->port_info.type(); if (!port_type) { - port_type = &(entry_it.second.value.type()); + port_type = &(entry->value.type()); } - std::cout << entry_it.first << " (" << demangle(port_type) << ") -> "; + std::cout << key << " (" << demangle(port_type) << ") -> "; if (auto parent = parent_bb_.lock()) { - auto remapping_it = internal_to_external_.find(entry_it.first); + auto remapping_it = internal_to_external_.find(key); 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::cout << ((entry->value.empty()) ? "empty" : "full") << std::endl; } } @@ -103,4 +93,55 @@ std::vector Blackboard::getKeys() const return out; } +std::shared_ptr +Blackboard::createEntryImpl(const std::string &key, const PortInfo& info) +{ + std::unique_lock lock(mutex_); + // This function might be called recursively, when we do remapping, because we move + // to the top scope to find already existing entries + + // search if exists already + auto storage_it = storage_.find(key); + if(storage_it != storage_.end()) + { + const auto& prev_info = storage_it->second->port_info; + if (prev_info.type() != info.type() && + prev_info.isStronglyTyped() && + info.isStronglyTyped()) + { + throw LogicError("Blackboard: once declared, the type of a port " + "shall not change. Previously declared type [", + BT::demangle(prev_info.type()), "] != new type [", + BT::demangle(info.type()), "]"); + } + return storage_it->second; + } + + std::shared_ptr entry; + + // manual remapping first + auto remapping_it = internal_to_external_.find(key); + if (remapping_it != internal_to_external_.end()) + { + const auto& remapped_key = remapping_it->second; + if (auto parent = parent_bb_.lock()) + { + entry = parent->createEntryImpl(remapped_key, info); + } + } + else if(autoremapping_) + { + if (auto parent = parent_bb_.lock()) + { + entry = parent->createEntryImpl(key, info); + } + } + else // not remapped, nor found. Create locally. + { + entry = std::make_shared(info); + } + storage_.insert( {key, entry} ); + return entry; +} + } // namespace BT diff --git a/src/bt_factory.cpp b/src/bt_factory.cpp index 62fd6f32b..7f32047a4 100644 --- a/src/bt_factory.cpp +++ b/src/bt_factory.cpp @@ -278,6 +278,13 @@ const std::set& BehaviorTreeFactory::builtinNodes() const Tree BehaviorTreeFactory::createTreeFromText(const std::string& text, Blackboard::Ptr blackboard) { + if(!parser_->registeredBehaviorTrees().empty()) { + std::cout << "WARNING: You executed BehaviorTreeFactory::createTreeFromText " + "after registerBehaviorTreeFrom[File/Text].\n" + "This is NOTm probably, what you want to do.\n" + "You should probably use BehaviorTreeFactory::createTree, instead" + << std::endl; + } XMLParser parser(*this); parser.loadFromText(text); auto tree = parser.instantiateTree(blackboard); @@ -288,6 +295,14 @@ Tree BehaviorTreeFactory::createTreeFromText(const std::string& text, Tree BehaviorTreeFactory::createTreeFromFile(const std::string& file_path, Blackboard::Ptr blackboard) { + if(!parser_->registeredBehaviorTrees().empty()) { + std::cout << "WARNING: You executed BehaviorTreeFactory::createTreeFromFile " + "after registerBehaviorTreeFrom[File/Text].\n" + "This is NOTm probably, what you want to do.\n" + "You should probably use BehaviorTreeFactory::createTree, instead" + << std::endl; + } + XMLParser parser(*this); parser.loadFromFile(file_path); auto tree = parser.instantiateTree(blackboard); diff --git a/src/control_node.cpp b/src/control_node.cpp index a25a2bdcb..4ec6a79fc 100644 --- a/src/control_node.cpp +++ b/src/control_node.cpp @@ -31,7 +31,19 @@ size_t ControlNode::childrenCount() const void ControlNode::halt() { - haltChildren(); + resetChildren(); +} + +void ControlNode::resetChildren() +{ + for (auto child: children_nodes_) + { + if (child->status() == NodeStatus::RUNNING) + { + child->halt(); + } + child->resetStatus(); + } } const std::vector& ControlNode::children() const diff --git a/src/controls/fallback_node.cpp b/src/controls/fallback_node.cpp index d7fb1df54..fc580d4f7 100644 --- a/src/controls/fallback_node.cpp +++ b/src/controls/fallback_node.cpp @@ -38,7 +38,7 @@ NodeStatus FallbackNode::tick() return child_status; } case NodeStatus::SUCCESS: { - haltChildren(); + resetChildren(); current_child_idx_ = 0; return child_status; } @@ -56,7 +56,7 @@ NodeStatus FallbackNode::tick() // The entire while loop completed. This means that all the children returned FAILURE. if (current_child_idx_ == children_count) { - haltChildren(); + resetChildren(); current_child_idx_ = 0; } diff --git a/src/controls/if_then_else_node.cpp b/src/controls/if_then_else_node.cpp index 7582f8de0..a5786ccae 100644 --- a/src/controls/if_then_else_node.cpp +++ b/src/controls/if_then_else_node.cpp @@ -71,7 +71,7 @@ NodeStatus IfThenElseNode::tick() } else { - haltChildren(); + resetChildren(); child_idx_ = 0; return status; } diff --git a/src/controls/parallel_node.cpp b/src/controls/parallel_node.cpp index 78ab08a16..bf1604388 100644 --- a/src/controls/parallel_node.cpp +++ b/src/controls/parallel_node.cpp @@ -97,7 +97,7 @@ NodeStatus ParallelNode::tick() if (success_childred_num == successThreshold()) { skip_list_.clear(); - haltChildren(); + resetChildren(); return NodeStatus::SUCCESS; } } @@ -116,7 +116,7 @@ NodeStatus ParallelNode::tick() (failure_childred_num == failureThreshold())) { skip_list_.clear(); - haltChildren(); + resetChildren(); return NodeStatus::FAILURE; } } diff --git a/src/controls/reactive_fallback.cpp b/src/controls/reactive_fallback.cpp index c89d43e00..d6d30d207 100644 --- a/src/controls/reactive_fallback.cpp +++ b/src/controls/reactive_fallback.cpp @@ -14,9 +14,22 @@ namespace BT { + +bool ReactiveFallback::throw_if_multiple_running = false; + +void ReactiveFallback::EnableException(bool enable) +{ + ReactiveFallback::throw_if_multiple_running = enable; +} + NodeStatus ReactiveFallback::tick() { size_t failure_count = 0; + if(status() == NodeStatus::IDLE) + { + running_child_ = -1; + } + setStatus(NodeStatus::RUNNING); for (size_t index = 0; index < childrenCount(); index++) { @@ -26,9 +39,23 @@ NodeStatus ReactiveFallback::tick() switch (child_status) { case NodeStatus::RUNNING: { - for (size_t i = index + 1; i < childrenCount(); i++) + // reset the previous children, to make sure that they are + // in IDLE state the next time we tick them + for (size_t i = 0; i < childrenCount(); i++) { - haltChild(i); + if(i != index) + { + haltChild(i); + } + } + if(running_child_ == -1) + { + running_child_ = int(index); + } + else if(throw_if_multiple_running && running_child_ != int(index)) + { + throw LogicError("[ReactiveFallback]: only a single child can return RUNNING.\n" + "This throw can be disabled with ReactiveFallback::EnableException(false)"); } return NodeStatus::RUNNING; } @@ -39,7 +66,7 @@ NodeStatus ReactiveFallback::tick() break; case NodeStatus::SUCCESS: { - haltChildren(); + resetChildren(); return NodeStatus::SUCCESS; } @@ -51,11 +78,17 @@ NodeStatus ReactiveFallback::tick() if (failure_count == childrenCount()) { - haltChildren(); + resetChildren(); return NodeStatus::FAILURE; } return NodeStatus::RUNNING; } +void ReactiveFallback::halt() +{ + running_child_ = -1; + ControlNode::halt(); +} + } // namespace BT diff --git a/src/controls/reactive_sequence.cpp b/src/controls/reactive_sequence.cpp index 2e1d81289..f4e00244e 100644 --- a/src/controls/reactive_sequence.cpp +++ b/src/controls/reactive_sequence.cpp @@ -14,10 +14,22 @@ namespace BT { + +bool ReactiveSequence::throw_if_multiple_running = false; + +void ReactiveSequence::EnableException(bool enable) +{ + ReactiveSequence::throw_if_multiple_running = enable; +} + NodeStatus ReactiveSequence::tick() { size_t success_count = 0; - size_t running_count = 0; + if(status() == NodeStatus::IDLE) + { + running_child_ = -1; + } + setStatus(NodeStatus::RUNNING); for (size_t index = 0; index < childrenCount(); index++) { @@ -27,17 +39,29 @@ NodeStatus ReactiveSequence::tick() switch (child_status) { case NodeStatus::RUNNING: { - running_count++; - - for (size_t i = index + 1; i < childrenCount(); i++) + // reset the previous children, to make sure that they are + // in IDLE state the next time we tick them + for (size_t i = 0; i < childrenCount(); i++) { - haltChild(i); + if(i != index) + { + haltChild(i); + } + } + if(running_child_ == -1) + { + running_child_ = int(index); + } + else if(throw_if_multiple_running && running_child_ != int(index)) + { + throw LogicError("[ReactiveSequence]: only a single child can return RUNNING.\n" + "This throw can be disabled with ReactiveSequence::EnableException(false)"); } return NodeStatus::RUNNING; } case NodeStatus::FAILURE: { - haltChildren(); + resetChildren(); return NodeStatus::FAILURE; } case NodeStatus::SUCCESS: { @@ -51,12 +75,19 @@ NodeStatus ReactiveSequence::tick() } // end switch } //end for + if (success_count == childrenCount()) { - haltChildren(); + resetChildren(); return NodeStatus::SUCCESS; } return NodeStatus::RUNNING; } +void ReactiveSequence::halt() +{ + running_child_ = -1; + ControlNode::halt(); +} + } // namespace BT diff --git a/src/controls/sequence_node.cpp b/src/controls/sequence_node.cpp index 11a224d51..9de146418 100644 --- a/src/controls/sequence_node.cpp +++ b/src/controls/sequence_node.cpp @@ -46,7 +46,7 @@ NodeStatus SequenceNode::tick() } case NodeStatus::FAILURE: { // Reset on failure - haltChildren(); + resetChildren(); current_child_idx_ = 0; return child_status; } @@ -64,7 +64,7 @@ NodeStatus SequenceNode::tick() // The entire while loop completed. This means that all the children returned SUCCESS. if (current_child_idx_ == children_count) { - haltChildren(); + resetChildren(); current_child_idx_ = 0; } return NodeStatus::SUCCESS; diff --git a/src/controls/sequence_star_node.cpp b/src/controls/sequence_star_node.cpp index f557611ca..bdd65c358 100644 --- a/src/controls/sequence_star_node.cpp +++ b/src/controls/sequence_star_node.cpp @@ -60,7 +60,7 @@ NodeStatus SequenceStarNode::tick() // The entire while loop completed. This means that all the children returned SUCCESS. if (current_child_idx_ == children_count) { - haltChildren(); + resetChildren(); current_child_idx_ = 0; } return NodeStatus::SUCCESS; @@ -68,7 +68,7 @@ NodeStatus SequenceStarNode::tick() void SequenceStarNode::halt() { - // DO NOT reset current_child_idx_ on halt + current_child_idx_ = 0; ControlNode::halt(); } diff --git a/src/controls/while_do_else_node.cpp b/src/controls/while_do_else_node.cpp index 543eefae4..b0ecae3cc 100644 --- a/src/controls/while_do_else_node.cpp +++ b/src/controls/while_do_else_node.cpp @@ -62,7 +62,7 @@ NodeStatus WhileDoElseNode::tick() } else { - haltChildren(); + resetChildren(); return status; } } diff --git a/src/decorator_node.cpp b/src/decorator_node.cpp index c239e0dbd..263e16bf0 100644 --- a/src/decorator_node.cpp +++ b/src/decorator_node.cpp @@ -31,7 +31,7 @@ void DecoratorNode::setChild(TreeNode* child) void DecoratorNode::halt() { - haltChild(); + resetChild(); } const TreeNode* DecoratorNode::child() const @@ -45,6 +45,11 @@ TreeNode* DecoratorNode::child() } void DecoratorNode::haltChild() +{ + resetChild(); +} + +void DecoratorNode::resetChild() { if (!child_node_) { @@ -57,6 +62,7 @@ void DecoratorNode::haltChild() child_node_->resetStatus(); } + SimpleDecoratorNode::SimpleDecoratorNode(const std::string& name, TickFunctor tick_functor, const NodeConfiguration& config) : diff --git a/src/decorators/delay_node.cpp b/src/decorators/delay_node.cpp index 2f68d4327..0be39ae12 100644 --- a/src/decorators/delay_node.cpp +++ b/src/decorators/delay_node.cpp @@ -42,13 +42,10 @@ NodeStatus DelayNode::tick() timer_id_ = timer_.add(std::chrono::milliseconds(msec_), [this](bool aborted) { std::unique_lock lk(delay_mutex_); - if (!aborted) + delay_complete_ = (!aborted); + if(!aborted) { - delay_complete_ = true; - } - else - { - delay_aborted_ = true; + emitStateChanged(); } }); } @@ -63,9 +60,13 @@ NodeStatus DelayNode::tick() } else if (delay_complete_) { - delay_started_ = false; - delay_aborted_ = false; auto child_status = child()->executeTick(); + if(child_status != NodeStatus::RUNNING) + { + delay_started_ = false; + delay_aborted_ = false; + resetChild(); + } return child_status; } else diff --git a/src/decorators/inverter_node.cpp b/src/decorators/inverter_node.cpp index caf60c815..a268c66ba 100644 --- a/src/decorators/inverter_node.cpp +++ b/src/decorators/inverter_node.cpp @@ -23,16 +23,17 @@ InverterNode::InverterNode(const std::string& name) : DecoratorNode(name, {}) NodeStatus InverterNode::tick() { setStatus(NodeStatus::RUNNING); + const NodeStatus child_status = child_node_->executeTick(); - const NodeStatus child_state = child_node_->executeTick(); - - switch (child_state) + switch (child_status) { case NodeStatus::SUCCESS: { + resetChild(); return NodeStatus::FAILURE; } case NodeStatus::FAILURE: { + resetChild(); return NodeStatus::SUCCESS; } @@ -44,7 +45,7 @@ NodeStatus InverterNode::tick() throw LogicError("A child node must never return IDLE"); } } - //return status(); + return status(); } } // namespace BT diff --git a/src/decorators/repeat_node.cpp b/src/decorators/repeat_node.cpp index 1aa0d4a31..1925765ca 100644 --- a/src/decorators/repeat_node.cpp +++ b/src/decorators/repeat_node.cpp @@ -53,13 +53,13 @@ NodeStatus RepeatNode::tick() { case NodeStatus::SUCCESS: { repeat_count_++; - haltChild(); + resetChild(); } break; case NodeStatus::FAILURE: { repeat_count_ = 0; - haltChild(); + resetChild(); return (NodeStatus::FAILURE); } diff --git a/src/decorators/retry_node.cpp b/src/decorators/retry_node.cpp index 7cea25487..3fb2e4740 100644 --- a/src/decorators/retry_node.cpp +++ b/src/decorators/retry_node.cpp @@ -58,13 +58,13 @@ NodeStatus RetryNode::tick() { case NodeStatus::SUCCESS: { try_count_ = 0; - haltChild(); + resetChild(); return (NodeStatus::SUCCESS); } case NodeStatus::FAILURE: { try_count_++; - haltChild(); + resetChild(); } break; diff --git a/src/decorators/subtree_node.cpp b/src/decorators/subtree_node.cpp index b3bb257f7..f5e2f0b97 100644 --- a/src/decorators/subtree_node.cpp +++ b/src/decorators/subtree_node.cpp @@ -12,7 +12,13 @@ BT::NodeStatus BT::SubtreeNode::tick() { setStatus(NodeStatus::RUNNING); } - return child_node_->executeTick(); + auto status = child_node_->executeTick(); + if(status != NodeStatus::RUNNING) + { + resetChild(); + } + + return status; } //-------------------------------- @@ -28,5 +34,11 @@ BT::NodeStatus BT::SubtreePlusNode::tick() { setStatus(NodeStatus::RUNNING); } - return child_node_->executeTick(); + auto status = child_node_->executeTick(); + if(status != NodeStatus::RUNNING) + { + resetChild(); + } + + return status; } diff --git a/src/tree_node.cpp b/src/tree_node.cpp index c6af29733..5cf7be9aa 100644 --- a/src/tree_node.cpp +++ b/src/tree_node.cpp @@ -78,8 +78,7 @@ void TreeNode::setStatus(NodeStatus new_status) void TreeNode::resetStatus() { - std::unique_lock lock(state_mutex_); - status_ = NodeStatus::IDLE; + setStatus(NodeStatus::IDLE); } NodeStatus TreeNode::status() const diff --git a/src/xml_parsing.cpp b/src/xml_parsing.cpp index 2ccad11cb..d6a8b8d4a 100644 --- a/src/xml_parsing.cpp +++ b/src/xml_parsing.cpp @@ -165,6 +165,7 @@ void XMLParser::Pimpl::loadDocImpl(tinyxml2::XMLDocument* doc, bool add_includes std::string ros_pkg_path; #ifdef USING_ROS ros_pkg_path = ros::package::getPath(ros_pkg_relative_path); + file_path = filesystem::path(ros_pkg_path) / file_path; #elif defined USING_ROS2 ros_pkg_path = ament_index_cpp::get_package_share_directory(ros_pkg_relative_path); @@ -522,7 +523,7 @@ TreeNode::Ptr XMLParser::Pimpl::createNodeFromXML(const XMLElement* element, for (const XMLAttribute* att = element->FirstAttribute(); att; att = att->Next()) { const std::string attribute_name = att->Name(); - if (ReservedPortNames.count(attribute_name) == 0) + if (!IsReservedPortname(attribute_name)) { port_remap[attribute_name] = att->Value(); } @@ -564,23 +565,24 @@ TreeNode::Ptr XMLParser::Pimpl::createNodeFromXML(const XMLElement* element, continue; } StringView param_value = remap_it->second; - auto param_res = TreeNode::getRemappedKey(port_name, param_value); - if (param_res) + + if (auto param_res = TreeNode::getRemappedKey(port_name, param_value)) { + // port_key will contain the key to find the entry in the blackboard const auto port_key = static_cast(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 + // if the entry already exists, check that the type is the same + if (auto prev_info = blackboard->portInfo(port_key)) { - // found. check consistency - if (prev_info->type() && - port_info.type() && // null type means that everything is valid - *prev_info->type() != *port_info.type()) + bool const port_type_mismatch = (prev_info->isStronglyTyped() && + port_info.isStronglyTyped() && + *prev_info->type() != *port_info.type()); + + // special case related to convertFromString + bool const string_input = ( prev_info->type() && + *prev_info->type() == typeid(std::string)); + + if (port_type_mismatch && !string_input) { blackboard->debugMessage(); @@ -590,6 +592,11 @@ TreeNode::Ptr XMLParser::Pimpl::createNodeFromXML(const XMLElement* element, demangle(port_info.type()), "] was used somewhere else."); } } + else + { + // not found, insert for the first time. + blackboard->createEntry(port_key, port_info); + } } } @@ -692,7 +699,7 @@ void BT::XMLParser::Pimpl::recursivelyCreateTree(const std::string& tree_ID, for (const XMLAttribute* attr = element->FirstAttribute(); attr != nullptr; attr = attr->Next()) { - if (ReservedPortNames.count(attr->Name()) == 0) + if (!IsReservedPortname(attr->Name())) { new_bb->addSubtreeRemapping(attr->Name(), attr->Value()); } @@ -707,21 +714,20 @@ void BT::XMLParser::Pimpl::recursivelyCreateTree(const std::string& tree_ID, output_tree.blackboard_stack.emplace_back(new_bb); std::set mapped_keys; - bool do_autoremap = false; - for (const XMLAttribute* attr = element->FirstAttribute(); attr != nullptr; attr = attr->Next()) { const char* attr_name = attr->Name(); const char* attr_value = attr->Value(); - if (ReservedPortNames.count(attr->Name()) != 0) + if (IsReservedPortname(attr->Name())) { continue; } if (StrEqual(attr_name, "__autoremap")) { - do_autoremap = convertFromString(attr_value); + bool do_autoremap = convertFromString(attr_value); + new_bb->enableAutoRemapping(do_autoremap); continue; } @@ -740,21 +746,6 @@ void BT::XMLParser::Pimpl::recursivelyCreateTree(const std::string& tree_ID, } } - if (do_autoremap) - { - std::vector remapped_ports; - auto new_root_element = tree_roots[node->name()]->FirstChildElement(); - - getPortsRecursively(new_root_element, remapped_ports); - for (const auto& port : remapped_ports) - { - if (mapped_keys.count(port) == 0) - { - new_bb->addSubtreeRemapping(port, port); - } - } - } - recursivelyCreateTree(node->name(), output_tree, new_bb, node); } } @@ -788,7 +779,7 @@ void XMLParser::Pimpl::getPortsRecursively(const XMLElement* element, { const char* attr_name = attr->Name(); const char* attr_value = attr->Value(); - if (ReservedPortNames.count(attr_name) == 0 && + if (!IsReservedPortname(attr_name) && TreeNode::isBlackboardPointer(attr_value)) { auto port_name = TreeNode::stripBlackboardPointer(attr_value); diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index f751d3e64..434f32985 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -1,7 +1,7 @@ ###################################################### # TESTS -include_directories(include) +include_directories(${PROJECT_SOURCE_DIR}/3rdparty include) set(BT_TESTS src/action_test_node.cpp @@ -15,6 +15,7 @@ set(BT_TESTS gtest_blackboard.cpp gtest_blackboard_precondition.cpp gtest_ports.cpp + gtest_reactive.cpp navigation_test.cpp gtest_subtree.cpp gtest_switch.cpp diff --git a/tests/gtest_blackboard.cpp b/tests/gtest_blackboard.cpp index 345bf7ba5..3341967eb 100644 --- a/tests/gtest_blackboard.cpp +++ b/tests/gtest_blackboard.cpp @@ -1,4 +1,4 @@ -/* Copyright (C) 2018-2019 Davide Faconti, Eurecat - All Rights Reserved +/* Copyright (C) 2018-2023 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, @@ -11,12 +11,10 @@ */ #include -#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" + +#include "../sample_nodes/dummy_nodes.h" using namespace BT; @@ -295,3 +293,37 @@ TEST(BlackboardTest, CheckTypeSafety) is = std::is_constructible::value; ASSERT_TRUE(is); } + +struct Point { + double x; + double y; +}; + +TEST(BlackboardTest, SetBlackboard_Issue725) +{ + BT::BehaviorTreeFactory factory; + + const std::string xml_text = R"( + + + + + )"; + + factory.registerNodeType("SaySomething"); + factory.registerBehaviorTreeFromText(xml_text); + auto tree = factory.createTree("MainTree"); + auto& blackboard = tree.blackboard_stack.front(); + + const Point point = {2,7}; + blackboard->set("first_point", point); + + const auto status = tree.tickRoot(); + + Point other_point = blackboard->get("other_point"); + + ASSERT_EQ(status, BT::NodeStatus::SUCCESS); + ASSERT_EQ(other_point.x, point.x); + ASSERT_EQ(other_point.y, point.y); +} + diff --git a/tests/gtest_fallback.cpp b/tests/gtest_fallback.cpp index 0ea701d30..d5d90e464 100644 --- a/tests/gtest_fallback.cpp +++ b/tests/gtest_fallback.cpp @@ -14,6 +14,7 @@ #include "action_test_node.h" #include "condition_test_node.h" #include "behaviortree_cpp_v3/behavior_tree.h" +#include "behaviortree_cpp_v3/bt_factory.h" using BT::NodeStatus; using std::chrono::milliseconds; @@ -148,8 +149,8 @@ TEST_F(ReactiveFallbackTest, Condition1ToTrue) 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::IDLE, condition_1.status()); + ASSERT_EQ(NodeStatus::IDLE, condition_2.status()); ASSERT_EQ(NodeStatus::RUNNING, action_1.status()); condition_1.setExpectedResult(NodeStatus::SUCCESS); @@ -170,8 +171,8 @@ TEST_F(ReactiveFallbackTest, Condition2ToTrue) 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::IDLE, condition_1.status()); + ASSERT_EQ(NodeStatus::IDLE, condition_2.status()); ASSERT_EQ(NodeStatus::RUNNING, action_1.status()); condition_2.setExpectedResult(NodeStatus::SUCCESS); @@ -310,3 +311,53 @@ TEST_F(ComplexFallbackWithMemoryTest, Action1Failed) ASSERT_EQ(NodeStatus::FAILURE, action_1.status()); ASSERT_EQ(NodeStatus::RUNNING, action_2.status()); } + + +TEST(FallbackAndRetry, FallbackAndRetry) +{ + using namespace BT; + static const char* xml_text = R"( + + + + + + + + + + + + + + )"; + + BehaviorTreeFactory factory; + factory.registerNodeType("AsyncActionTest"); + + auto tree = factory.createTreeFromText(xml_text); + + std::vector async; + + for(auto node: tree.nodes) { + if(auto async_node = dynamic_cast(node.get()) ) + { + async.push_back(async_node); + } + } + + ASSERT_EQ(async.size(), 2); + async[0]->setExpectedResult(NodeStatus::FAILURE); + async[1]->setExpectedResult(NodeStatus::SUCCESS); + + auto res = tree.tickRootWhileRunning(); + + ASSERT_EQ(async[0]->failureCount(), 2); + ASSERT_EQ(async[0]->successCount(), 0); + + ASSERT_EQ(async[1]->failureCount(), 0); + ASSERT_EQ(async[1]->successCount(), 2); + + ASSERT_EQ(NodeStatus::FAILURE, res); +} + diff --git a/tests/gtest_reactive.cpp b/tests/gtest_reactive.cpp new file mode 100644 index 000000000..b7df2b6c7 --- /dev/null +++ b/tests/gtest_reactive.cpp @@ -0,0 +1,69 @@ +#include +#include "behaviortree_cpp_v3/bt_factory.h" +#include "test_helper.hpp" +#include "behaviortree_cpp_v3/loggers/bt_cout_logger.h" + +using BT::NodeStatus; +using std::chrono::milliseconds; + +class SleepNode : public BT::StatefulActionNode +{ +public: + + SleepNode(const std::string& name, const BT::NodeConfiguration& config): + StatefulActionNode(name, config) {} + + NodeStatus onStart() override { + count_ = 0; + return NodeStatus::RUNNING; + } + + NodeStatus onRunning() override { + return ++count_ < 10 ? NodeStatus::RUNNING : NodeStatus::SUCCESS; + } + + void onHalted() override {} + + static BT::PortsList providedPorts(){ + return {}; + } + +private: + int count_ = 0; +}; + + +TEST(Reactive, TestLogging) +{ + using namespace BT; + + static const char* reactive_xml_text = R"( + + + + + + + + + +)"; + + BehaviorTreeFactory factory; + + factory.registerNodeType("Sleep"); + + std::array counters; + RegisterTestTick(factory, "Test", counters); + + auto tree = factory.createTreeFromText(reactive_xml_text); + StdCoutLogger logger(tree); + + auto ret = tree.tickRootWhileRunning(); + ASSERT_EQ(ret, NodeStatus::SUCCESS); + + int num_ticks = counters[0]; + ASSERT_GE(num_ticks, 10); +} + + diff --git a/tests/gtest_sequence.cpp b/tests/gtest_sequence.cpp index 6ac5c6bf7..5c49548c0 100644 --- a/tests/gtest_sequence.cpp +++ b/tests/gtest_sequence.cpp @@ -226,7 +226,7 @@ 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, seq_conditions.status()); ASSERT_EQ(NodeStatus::IDLE, condition_1.status()); ASSERT_EQ(NodeStatus::IDLE, condition_1.status()); ASSERT_EQ(NodeStatus::RUNNING, action_1.status()); @@ -386,143 +386,3 @@ TEST_F(ComplexSequenceWithMemoryTest, Conditions1ToFalse) 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()); -} - -TEST_F(ComplexSequenceWithMemoryTest, Action2FailureSeq) -{ - root.executeTick(); - 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()); - - action_2.setExpectedResult(NodeStatus::FAILURE); - std::this_thread::sleep_for(milliseconds(150)); - root.executeTick(); - - // failure in action_2 does not affect the state of already - // executed nodes (seq_conditions and action_1) - ASSERT_EQ(NodeStatus::FAILURE, root.status()); - ASSERT_EQ(NodeStatus::SUCCESS, 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::SUCCESS, action_1.status()); - ASSERT_EQ(NodeStatus::IDLE, action_2.status()); - - action_2.setExpectedResult(NodeStatus::SUCCESS); - 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()); -} - -TEST_F(ComplexSequenceWithMemoryTest, Action2HaltSeq) -{ - root.executeTick(); - std::this_thread::sleep_for(milliseconds(150)); - root.executeTick(); - - root.halt(); - - 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()); - - root.executeTick(); - - // tree retakes execution from action_2 - ASSERT_EQ(NodeStatus::IDLE, 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::IDLE, 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 index 6410cf688..5074bbcda 100644 --- a/tests/gtest_subtree.cpp +++ b/tests/gtest_subtree.cpp @@ -1,6 +1,7 @@ #include #include "behaviortree_cpp_v3/bt_factory.h" #include "../sample_nodes/dummy_nodes.h" +#include "../sample_nodes/movebase_node.h" using namespace BT; @@ -20,7 +21,7 @@ TEST(SubTree, SiblingPorts_Issue_72) - + )"; @@ -146,16 +147,13 @@ TEST(SubTree, SubtreePlusA) - - - - + )"; @@ -310,3 +308,182 @@ TEST(SubTree, SubtreeIssue433) ASSERT_EQ(ret, BT::NodeStatus::SUCCESS); } + + +class NaughtyNav2Node : public BT::SyncActionNode +{ +public: + NaughtyNav2Node(const std::string& name, const BT::NodeConfiguration& config) : + BT::SyncActionNode(name, config) + { + std::cout << "CTOR:" << config.blackboard->get("ros_node") << std::endl; + } + + BT::NodeStatus tick() override + { + std::cout << "tick:" << config().blackboard->get("ros_node") << std::endl; + return BT::NodeStatus::SUCCESS; + } + static BT::PortsList providedPorts() + { + return {}; + } +}; + +TEST(SubTree, SubtreeNav2_Issue563) +{ + static const char* xml_text = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + + BehaviorTreeFactory factory; + factory.registerNodeType("SaySomething"); + factory.registerNodeType("NaughtyNav2Node"); + + auto blackboard = BT::Blackboard::create(); + blackboard->set("ros_node", "nav2_shouldnt_do_this"); + + Tree tree = factory.createTreeFromText(xml_text, blackboard); + + auto ret = tree.tickRoot(); + ASSERT_EQ(ret, NodeStatus::SUCCESS); +} + +TEST(SubTree, SubtreeNav2_Issue724) +{ + static const char* xml_text = R"( + + + + + + + + + + + + + + + + + + + + + + +)"; + + BehaviorTreeFactory factory; + factory.registerNodeType("NaughtyNav2Node"); + + factory.registerBehaviorTreeFromText(xml_text); + + auto blackboard = BT::Blackboard::create(); + blackboard->set("ros_node", "nav2_shouldnt_do_this"); + + Tree tree = factory.createTreeFromText(xml_text, blackboard); + + auto ret = tree.tickRoot(); + ASSERT_EQ(ret, NodeStatus::SUCCESS); +} + +TEST(SubTree, String_to_Pose_Issue623) +{ + // clang-format off + + static const char* xml_text = R"( + + + + + + + + + + + + + )"; + + // clang-format on + + BehaviorTreeFactory factory; + factory.registerNodeType("MoveBase"); + auto tree = factory.createTreeFromText(xml_text); + tree.tickRootWhileRunning(); +} + +class Assert : public BT::SyncActionNode +{ +public: + Assert(const std::string& name, const BT::NodeConfiguration& config) + : BT::SyncActionNode(name, config) {} + + static BT::PortsList providedPorts() { + return {BT::InputPort("condition")}; + } + +private: + virtual BT::NodeStatus tick() override { + if (getInput("condition").value()) + return BT::NodeStatus::SUCCESS; + else + return BT::NodeStatus::FAILURE; + } +}; + +TEST(SubTree, Issue653_SetBlackboard) +{ + // clang-format off + + static const char* xml_text = R"( + + + + + + + + + + + + + )"; + + // clang-format on + + BehaviorTreeFactory factory; + factory.registerNodeType("Assert"); + auto tree = factory.createTreeFromText(xml_text); + tree.tickRootWhileRunning(); +} diff --git a/tests/test_helper.hpp b/tests/test_helper.hpp new file mode 100644 index 000000000..514923c0a --- /dev/null +++ b/tests/test_helper.hpp @@ -0,0 +1,27 @@ +#ifndef TEST_HELPER_HPP +#define TEST_HELPER_HPP + +#include "behaviortree_cpp_v3/bt_factory.h" + +inline BT::NodeStatus TestTick(int* tick_counter) +{ + (*tick_counter)++; + return BT::NodeStatus::SUCCESS; +} + +template inline + void RegisterTestTick(BT::BehaviorTreeFactory& factory, const std::string& name_prefix, + std::array& tick_counters) +{ + for(size_t i=0; i