diff --git a/plansys2_bt_actions/CMakeLists.txt b/plansys2_bt_actions/CMakeLists.txt index 8879b22ed..efed76722 100644 --- a/plansys2_bt_actions/CMakeLists.txt +++ b/plansys2_bt_actions/CMakeLists.txt @@ -9,18 +9,10 @@ find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(rclcpp_lifecycle REQUIRED) find_package(plansys2_executor REQUIRED) -find_package(behaviortree_cpp_v3 REQUIRED) +find_package(behaviortree_cpp REQUIRED) find_package(action_msgs REQUIRED) find_package(lifecycle_msgs REQUIRED) -find_package(ZMQ) -if(ZMQ_FOUND) - message(STATUS "ZeroMQ found.") - add_definitions(-DZMQ_FOUND) -else() - message(WARNING "ZeroMQ NOT found. Not including PublisherZMQ.") -endif() - set(CMAKE_CXX_STANDARD 17) set(dependencies @@ -28,12 +20,12 @@ set(dependencies rclcpp_action rclcpp_lifecycle plansys2_executor - behaviortree_cpp_v3 + behaviortree_cpp action_msgs lifecycle_msgs ) -include_directories(include ${ZMQ_INCLUDE_DIRS}) +include_directories(include) set(BT_ACTIONS_SOURCES src/plansys2_bt_actions/BTAction.cpp @@ -41,7 +33,6 @@ set(BT_ACTIONS_SOURCES add_library(${PROJECT_NAME} SHARED ${BT_ACTIONS_SOURCES}) ament_target_dependencies(${PROJECT_NAME} ${dependencies}) -target_link_libraries(${PROJECT_NAME} ${ZMQ_LIBRARIES}) add_executable(bt_action_node src/bt_action_node.cpp @@ -77,5 +68,6 @@ endif() ament_export_include_directories(include) ament_export_dependencies(${dependencies}) +include_directories(include) ament_package() diff --git a/plansys2_bt_actions/README.md b/plansys2_bt_actions/README.md index f7f0595fa..1ab8e63a7 100644 --- a/plansys2_bt_actions/README.md +++ b/plansys2_bt_actions/README.md @@ -20,7 +20,7 @@ Files created by the `.fbl` and minitrace loggers are stored in `/tmp/ -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# -# * Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in the -# documentation and/or other materials provided with the distribution. -# -# * Neither the name of the copyright holder nor the names of its -# contributors may be used to endorse or promote products derived from -# this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Original license text (changed to the above to pass ament_copyright): -# 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() - - 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() - - 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() diff --git a/plansys2_bt_actions/include/plansys2_bt_actions/BTAction.hpp b/plansys2_bt_actions/include/plansys2_bt_actions/BTAction.hpp index 1864bf192..e7e015edb 100644 --- a/plansys2_bt_actions/include/plansys2_bt_actions/BTAction.hpp +++ b/plansys2_bt_actions/include/plansys2_bt_actions/BTAction.hpp @@ -19,15 +19,11 @@ #include #include -#include "behaviortree_cpp_v3/behavior_tree.h" -#include "behaviortree_cpp_v3/bt_factory.h" -#include "behaviortree_cpp_v3/xml_parsing.h" -#include "behaviortree_cpp_v3/loggers/bt_file_logger.h" -#include "behaviortree_cpp_v3/loggers/bt_minitrace_logger.h" - -#ifdef ZMQ_FOUND -#include -#endif +#include "behaviortree_cpp/behavior_tree.h" +#include "behaviortree_cpp/bt_factory.h" +#include "behaviortree_cpp/xml_parsing.h" +#include "behaviortree_cpp/loggers/bt_file_logger_v2.h" +#include "behaviortree_cpp/loggers/bt_minitrace_logger.h" #include "plansys2_executor/ActionExecutorClient.hpp" #include "rclcpp/rclcpp.hpp" @@ -69,8 +65,7 @@ class BTAction : public plansys2::ActionExecutorClient std::string bt_xml_file_; std::vector plugin_list_; bool finished_; - std::unique_ptr publisher_zmq_; - std::unique_ptr bt_file_logger_; + std::unique_ptr bt_file_logger_; std::unique_ptr bt_minitrace_logger_; }; diff --git a/plansys2_bt_actions/include/plansys2_bt_actions/BTActionNode.hpp b/plansys2_bt_actions/include/plansys2_bt_actions/BTActionNode.hpp index 3ac832ba4..d1edc4d82 100644 --- a/plansys2_bt_actions/include/plansys2_bt_actions/BTActionNode.hpp +++ b/plansys2_bt_actions/include/plansys2_bt_actions/BTActionNode.hpp @@ -18,7 +18,7 @@ #include #include -#include "behaviortree_cpp_v3/action_node.h" +#include "behaviortree_cpp/action_node.h" #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "rclcpp_action/rclcpp_action.hpp" @@ -35,10 +35,12 @@ class BtActionNode : public BT::ActionNodeBase BtActionNode( const std::string & xml_tag_name, const std::string & action_name, - const BT::NodeConfiguration & conf) + const BT::NodeConfig & conf) : BT::ActionNodeBase(xml_tag_name, conf), action_name_(action_name) { - config().blackboard->get("node", node_); + if (!config().blackboard->get("node", node_)) { + RCLCPP_ERROR(node_->get_logger(), "Failed to get 'node' from the blackboard"); + } // Get the required items from the blackboard server_timeout_ = 5s; @@ -95,6 +97,7 @@ class BtActionNode : public BT::ActionNodeBase 5.0, "The amount of time to wait for a response from the action server, in seconds") }; + // The user defined ports are added to the basic ports basic.insert(addition.begin(), addition.end()); return basic; @@ -307,7 +310,7 @@ class BtActionNode : public BT::ActionNodeBase cancel_goal(); } - setStatus(BT::NodeStatus::IDLE); + resetStatus(); } protected: diff --git a/plansys2_bt_actions/package.xml b/plansys2_bt_actions/package.xml index 4f7048d1a..a3fda8211 100644 --- a/plansys2_bt_actions/package.xml +++ b/plansys2_bt_actions/package.xml @@ -16,7 +16,7 @@ rclcpp_action rclcpp_lifecycle plansys2_executor - behaviortree_cpp_v3 + behaviortree_cpp action_msgs libzmq3-dev diff --git a/plansys2_bt_actions/src/plansys2_bt_actions/BTAction.cpp b/plansys2_bt_actions/src/plansys2_bt_actions/BTAction.cpp index 2509015f0..84e2531a8 100644 --- a/plansys2_bt_actions/src/plansys2_bt_actions/BTAction.cpp +++ b/plansys2_bt_actions/src/plansys2_bt_actions/BTAction.cpp @@ -22,7 +22,7 @@ #include #include -#include "behaviortree_cpp_v3/utils/shared_library.h" +#include "behaviortree_cpp/utils/shared_library.h" #include "plansys2_bt_actions/BTAction.hpp" namespace plansys2 @@ -38,12 +38,6 @@ BTAction::BTAction( "plugins", std::vector({})); declare_parameter("bt_file_logging", false); declare_parameter("bt_minitrace_logging", false); -#ifdef ZMQ_FOUND - declare_parameter("enable_groot_monitoring", true); - declare_parameter("publisher_port", -1); - declare_parameter("server_port", -1); - declare_parameter("max_msgs_per_second", 25); -#endif } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn @@ -75,7 +69,6 @@ BTAction::on_configure(const rclcpp_lifecycle::State & previous_state) rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn BTAction::on_cleanup(const rclcpp_lifecycle::State & previous_state) { - publisher_zmq_.reset(); return ActionExecutorClient::on_cleanup(previous_state); } @@ -93,8 +86,12 @@ BTAction::on_activate(const rclcpp_lifecycle::State & previous_state) } for (int i = 0; i < get_arguments().size(); i++) { + auto arg = get_arguments()[i]; + RCLCPP_DEBUG_STREAM( + get_logger(), + "Setting arg" << i << " [" << arg << "]"); std::string argname = "arg" + std::to_string(i); - blackboard_->set(argname, get_arguments()[i]); + blackboard_->set(argname, arg); } if (get_parameter("bt_file_logging").as_bool() || @@ -116,7 +113,7 @@ BTAction::on_activate(const rclcpp_lifecycle::State & previous_state) get_logger(), "Logging to file: " << filename_extension); bt_file_logger_ = - std::make_unique(tree_, filename_extension.c_str()); + std::make_unique(tree_, filename_extension.c_str()); } if (get_parameter("bt_minitrace_logging").as_bool()) { @@ -129,36 +126,6 @@ BTAction::on_activate(const rclcpp_lifecycle::State & previous_state) } } -#ifdef ZMQ_FOUND - bool enable_groot_monitoring = get_parameter("enable_groot_monitoring").as_bool(); - int publisher_port = get_parameter("publisher_port").as_int(); - int server_port = get_parameter("server_port").as_int(); - unsigned int max_msgs_per_second = get_parameter("max_msgs_per_second").as_int(); - - if (enable_groot_monitoring) { - if (publisher_port <= 0 || server_port <= 0) { - RCLCPP_WARN( - get_logger(), - "[%s] Groot monitoring ports not provided, disabling Groot monitoring." - " publisher port: %d, server port: %d", - get_name(), publisher_port, server_port); - } else { - RCLCPP_DEBUG( - get_logger(), - "[%s] Groot monitoring: Publisher port: %d, Server port: %d, Max msgs per second: %d", - get_name(), publisher_port, server_port, max_msgs_per_second); - try { - publisher_zmq_.reset( - new BT::PublisherZMQ( - tree_, max_msgs_per_second, publisher_port, - server_port)); - } catch (const BT::LogicError & exc) { - RCLCPP_ERROR(get_logger(), "ZMQ error: %s", exc.what()); - } - } - } -#endif - finished_ = false; return ActionExecutorClient::on_activate(previous_state); } @@ -166,7 +133,6 @@ BTAction::on_activate(const rclcpp_lifecycle::State & previous_state) rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn BTAction::on_deactivate(const rclcpp_lifecycle::State & previous_state) { - publisher_zmq_.reset(); bt_minitrace_logger_.reset(); bt_file_logger_.reset(); tree_.haltTree(); diff --git a/plansys2_bt_actions/test/behavior_tree/CloseGripper.cpp b/plansys2_bt_actions/test/behavior_tree/CloseGripper.cpp index 33a7eef7d..434dfc943 100644 --- a/plansys2_bt_actions/test/behavior_tree/CloseGripper.cpp +++ b/plansys2_bt_actions/test/behavior_tree/CloseGripper.cpp @@ -17,14 +17,14 @@ #include "CloseGripper.hpp" -#include "behaviortree_cpp_v3/behavior_tree.h" +#include "behaviortree_cpp/behavior_tree.h" namespace plansys2_bt_tests { CloseGripper::CloseGripper( const std::string & xml_tag_name, - const BT::NodeConfiguration & conf) + const BT::NodeConfig & conf) : BT::ActionNodeBase(xml_tag_name, conf), counter_(0) { } @@ -50,7 +50,7 @@ CloseGripper::tick() } // namespace plansys2_bt_tests -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("CloseGripper"); diff --git a/plansys2_bt_actions/test/behavior_tree/CloseGripper.hpp b/plansys2_bt_actions/test/behavior_tree/CloseGripper.hpp index 60b433880..3b496ed38 100644 --- a/plansys2_bt_actions/test/behavior_tree/CloseGripper.hpp +++ b/plansys2_bt_actions/test/behavior_tree/CloseGripper.hpp @@ -17,8 +17,8 @@ #include -#include "behaviortree_cpp_v3/behavior_tree.h" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/behavior_tree.h" +#include "behaviortree_cpp/bt_factory.h" namespace plansys2_bt_tests { @@ -28,7 +28,7 @@ class CloseGripper : public BT::ActionNodeBase public: explicit CloseGripper( const std::string & xml_tag_name, - const BT::NodeConfiguration & conf); + const BT::NodeConfig & conf); void halt(); BT::NodeStatus tick(); diff --git a/plansys2_bt_actions/test/behavior_tree/FailureNodes.cpp b/plansys2_bt_actions/test/behavior_tree/FailureNodes.cpp index 9cc2fa8e2..1dc633b20 100644 --- a/plansys2_bt_actions/test/behavior_tree/FailureNodes.cpp +++ b/plansys2_bt_actions/test/behavior_tree/FailureNodes.cpp @@ -15,14 +15,14 @@ #include #include -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "FailureNodes.hpp" BT_REGISTER_NODES(factory) { BT::NodeBuilder builder = - [](const std::string & name, const BT::NodeConfiguration & config) + [](const std::string & name, const BT::NodeConfig & config) { return std::make_unique( name, "move", config); @@ -32,7 +32,7 @@ BT_REGISTER_NODES(factory) "OnTickFail", builder); builder = - [](const std::string & name, const BT::NodeConfiguration & config) + [](const std::string & name, const BT::NodeConfig & config) { return std::make_unique( name, "move", config); diff --git a/plansys2_bt_actions/test/behavior_tree/FailureNodes.hpp b/plansys2_bt_actions/test/behavior_tree/FailureNodes.hpp index f495a8e5e..0156af75e 100644 --- a/plansys2_bt_actions/test/behavior_tree/FailureNodes.hpp +++ b/plansys2_bt_actions/test/behavior_tree/FailureNodes.hpp @@ -32,7 +32,7 @@ class OnTickFail : public plansys2::BtActionNode explicit OnTickFail( const std::string & xml_tag_name, const std::string & action_name, - const BT::NodeConfiguration & conf) + const BT::NodeConfig & conf) : plansys2::BtActionNode(xml_tag_name, action_name, conf), @@ -57,7 +57,7 @@ class OnFeedbackFail : public plansys2::BtActionNode(xml_tag_name, action_name, conf), diff --git a/plansys2_bt_actions/test/behavior_tree/Move.cpp b/plansys2_bt_actions/test/behavior_tree/Move.cpp index 6d87b9d6a..9a4b6a6f0 100644 --- a/plansys2_bt_actions/test/behavior_tree/Move.cpp +++ b/plansys2_bt_actions/test/behavior_tree/Move.cpp @@ -21,7 +21,7 @@ #include "geometry_msgs/msg/pose2_d.hpp" -#include "behaviortree_cpp_v3/behavior_tree.h" +#include "behaviortree_cpp/behavior_tree.h" namespace plansys2_bt_tests { @@ -29,11 +29,13 @@ namespace plansys2_bt_tests Move::Move( const std::string & xml_tag_name, const std::string & action_name, - const BT::NodeConfiguration & conf) + const BT::NodeConfig & conf) : plansys2::BtActionNode(xml_tag_name, action_name, conf) { rclcpp_lifecycle::LifecycleNode::SharedPtr node; - config().blackboard->get("node", node); + if (!config().blackboard->get("node", node)) { + RCLCPP_ERROR(node_->get_logger(), "Failed to get 'node' from the blackboard"); + } node->declare_parameter>( "waypoints", std::vector({})); @@ -95,11 +97,11 @@ Move::on_success() } // namespace plansys2_bt_tests -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { BT::NodeBuilder builder = - [](const std::string & name, const BT::NodeConfiguration & config) + [](const std::string & name, const BT::NodeConfig & config) { return std::make_unique( name, "move", config); diff --git a/plansys2_bt_actions/test/behavior_tree/Move.hpp b/plansys2_bt_actions/test/behavior_tree/Move.hpp index 9f807fbd2..bd2674197 100644 --- a/plansys2_bt_actions/test/behavior_tree/Move.hpp +++ b/plansys2_bt_actions/test/behavior_tree/Move.hpp @@ -22,8 +22,8 @@ #include "test_msgs/action/fibonacci.hpp" #include "plansys2_bt_actions/BTActionNode.hpp" -#include "behaviortree_cpp_v3/behavior_tree.h" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/behavior_tree.h" +#include "behaviortree_cpp/bt_factory.h" namespace plansys2_bt_tests { @@ -34,7 +34,7 @@ class Move : public plansys2::BtActionNode explicit Move( const std::string & xml_tag_name, const std::string & action_name, - const BT::NodeConfiguration & conf); + const BT::NodeConfig & conf); BT::NodeStatus on_tick() override; BT::NodeStatus on_success() override; diff --git a/plansys2_bt_actions/test/behavior_tree/OpenGripper.cpp b/plansys2_bt_actions/test/behavior_tree/OpenGripper.cpp index 45b1dd974..4c48dab98 100644 --- a/plansys2_bt_actions/test/behavior_tree/OpenGripper.cpp +++ b/plansys2_bt_actions/test/behavior_tree/OpenGripper.cpp @@ -17,14 +17,14 @@ #include "OpenGripper.hpp" -#include "behaviortree_cpp_v3/behavior_tree.h" +#include "behaviortree_cpp/behavior_tree.h" namespace plansys2_bt_tests { OpenGripper::OpenGripper( const std::string & xml_tag_name, - const BT::NodeConfiguration & conf) + const BT::NodeConfig & conf) : BT::ActionNodeBase(xml_tag_name, conf), counter_(0) { } @@ -50,7 +50,7 @@ OpenGripper::tick() } // namespace plansys2_bt_tests -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("OpenGripper"); diff --git a/plansys2_bt_actions/test/behavior_tree/OpenGripper.hpp b/plansys2_bt_actions/test/behavior_tree/OpenGripper.hpp index dc64923a6..da5f7d239 100644 --- a/plansys2_bt_actions/test/behavior_tree/OpenGripper.hpp +++ b/plansys2_bt_actions/test/behavior_tree/OpenGripper.hpp @@ -17,8 +17,8 @@ #include -#include "behaviortree_cpp_v3/behavior_tree.h" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/behavior_tree.h" +#include "behaviortree_cpp/bt_factory.h" namespace plansys2_bt_tests { @@ -28,7 +28,7 @@ class OpenGripper : public BT::ActionNodeBase public: explicit OpenGripper( const std::string & xml_tag_name, - const BT::NodeConfiguration & conf); + const BT::NodeConfig & conf); void halt(); BT::NodeStatus tick(); diff --git a/plansys2_bt_actions/test/unit/bt_action_test.cpp b/plansys2_bt_actions/test/unit/bt_action_test.cpp index 9a81a0b71..a81987be2 100644 --- a/plansys2_bt_actions/test/unit/bt_action_test.cpp +++ b/plansys2_bt_actions/test/unit/bt_action_test.cpp @@ -18,9 +18,9 @@ #include #include -#include "behaviortree_cpp_v3/behavior_tree.h" -#include "behaviortree_cpp_v3/bt_factory.h" -#include "behaviortree_cpp_v3/utils/shared_library.h" +#include "behaviortree_cpp/behavior_tree.h" +#include "behaviortree_cpp/bt_factory.h" +#include "behaviortree_cpp/utils/shared_library.h" #include "../behavior_tree/OpenGripper.hpp" #include "../behavior_tree/CloseGripper.hpp" @@ -164,7 +164,7 @@ TEST(bt_actions, on_tick_failure) }); - BT::NodeConfiguration config; + BT::NodeConfig config; BT::assignDefaultRemapping(config); auto bb = BT::Blackboard::create(); bb->set("node", node); @@ -201,7 +201,7 @@ TEST(bt_actions, on_feedback_failure) } }); - BT::NodeConfiguration config; + BT::NodeConfig config; BT::assignDefaultRemapping(config); auto bb = BT::Blackboard::create(); bb->set("node", node); diff --git a/plansys2_executor/CMakeLists.txt b/plansys2_executor/CMakeLists.txt index 5c7231a70..4647fcbcf 100644 --- a/plansys2_executor/CMakeLists.txt +++ b/plansys2_executor/CMakeLists.txt @@ -20,18 +20,10 @@ find_package(plansys2_domain_expert REQUIRED) find_package(plansys2_problem_expert REQUIRED) find_package(plansys2_planner REQUIRED) find_package(pluginlib REQUIRED) -find_package(behaviortree_cpp_v3 REQUIRED) +find_package(behaviortree_cpp REQUIRED) find_package(std_msgs REQUIRED) find_package(std_srvs REQUIRED) -find_package(ZMQ) -if(ZMQ_FOUND) - message(STATUS "ZeroMQ found.") - add_definitions(-DZMQ_FOUND) -else() - message(WARNING "ZeroMQ NOT found. Not including PublisherZMQ.") -endif() - set(CMAKE_CXX_STANDARD 17) set(dependencies @@ -50,12 +42,12 @@ set(dependencies plansys2_problem_expert plansys2_planner pluginlib - behaviortree_cpp_v3 + behaviortree_cpp std_msgs std_srvs ) -include_directories(include ${ZMQ_INCLUDE_DIRS}) +include_directories(include) set(EXECUTOR_SOURCES src/plansys2_executor/ExecutorClient.cpp @@ -77,7 +69,7 @@ set(EXECUTOR_SOURCES add_library(${PROJECT_NAME} SHARED ${EXECUTOR_SOURCES}) ament_target_dependencies(${PROJECT_NAME} ${dependencies}) -target_link_libraries(${PROJECT_NAME} ${ZMQ_LIBRARIES}) +target_link_libraries(${PROJECT_NAME}) add_executable(executor_node src/executor_node.cpp diff --git a/plansys2_executor/cmake/FindZMQ.cmake b/plansys2_executor/cmake/FindZMQ.cmake deleted file mode 100644 index cb2e4e6a2..000000000 --- a/plansys2_executor/cmake/FindZMQ.cmake +++ /dev/null @@ -1,84 +0,0 @@ -# - 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 -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# -# * Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in the -# documentation and/or other materials provided with the distribution. -# -# * Neither the name of the copyright holder nor the names of its -# contributors may be used to endorse or promote products derived from -# this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Original license text (changed to the above to pass ament_copyright): -# 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() - - 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() - - 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() diff --git a/plansys2_executor/include/plansys2_executor/ActionExecutor.hpp b/plansys2_executor/include/plansys2_executor/ActionExecutor.hpp index 73997d720..912b472df 100644 --- a/plansys2_executor/include/plansys2_executor/ActionExecutor.hpp +++ b/plansys2_executor/include/plansys2_executor/ActionExecutor.hpp @@ -22,7 +22,7 @@ #include "plansys2_msgs/msg/action_execution.hpp" #include "plansys2_msgs/msg/action_execution_info.hpp" #include "plansys2_msgs/msg/durative_action.hpp" -#include "behaviortree_cpp_v3/behavior_tree.h" +#include "behaviortree_cpp/behavior_tree.h" #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" diff --git a/plansys2_executor/include/plansys2_executor/behavior_tree/apply_atend_effect_node.hpp b/plansys2_executor/include/plansys2_executor/behavior_tree/apply_atend_effect_node.hpp index 12fdf34db..1d8043ab2 100644 --- a/plansys2_executor/include/plansys2_executor/behavior_tree/apply_atend_effect_node.hpp +++ b/plansys2_executor/include/plansys2_executor/behavior_tree/apply_atend_effect_node.hpp @@ -19,7 +19,7 @@ #include #include -#include "behaviortree_cpp_v3/action_node.h" +#include "behaviortree_cpp/action_node.h" #include "plansys2_problem_expert/ProblemExpertClient.hpp" #include "plansys2_executor/ActionExecutor.hpp" @@ -35,7 +35,7 @@ class ApplyAtEndEffect : public BT::ActionNodeBase public: ApplyAtEndEffect( const std::string & xml_tag_name, - const BT::NodeConfiguration & conf); + const BT::NodeConfig & conf); void halt() {} BT::NodeStatus tick() override; diff --git a/plansys2_executor/include/plansys2_executor/behavior_tree/apply_atstart_effect_node.hpp b/plansys2_executor/include/plansys2_executor/behavior_tree/apply_atstart_effect_node.hpp index 200a66603..b0dee8f9c 100644 --- a/plansys2_executor/include/plansys2_executor/behavior_tree/apply_atstart_effect_node.hpp +++ b/plansys2_executor/include/plansys2_executor/behavior_tree/apply_atstart_effect_node.hpp @@ -19,7 +19,7 @@ #include #include -#include "behaviortree_cpp_v3/action_node.h" +#include "behaviortree_cpp/action_node.h" #include "plansys2_problem_expert/ProblemExpertClient.hpp" #include "plansys2_executor/ActionExecutor.hpp" @@ -35,7 +35,7 @@ class ApplyAtStartEffect : public BT::ActionNodeBase public: ApplyAtStartEffect( const std::string & xml_tag_name, - const BT::NodeConfiguration & conf); + const BT::NodeConfig & conf); void halt() {} BT::NodeStatus tick() override; diff --git a/plansys2_executor/include/plansys2_executor/behavior_tree/check_action_node.hpp b/plansys2_executor/include/plansys2_executor/behavior_tree/check_action_node.hpp index 150c9cb5d..dcc100ffc 100644 --- a/plansys2_executor/include/plansys2_executor/behavior_tree/check_action_node.hpp +++ b/plansys2_executor/include/plansys2_executor/behavior_tree/check_action_node.hpp @@ -20,7 +20,7 @@ #include -#include "behaviortree_cpp_v3/action_node.h" +#include "behaviortree_cpp/action_node.h" #include "plansys2_executor/ActionExecutor.hpp" @@ -36,7 +36,7 @@ class CheckAction : public BT::ActionNodeBase public: CheckAction( const std::string & xml_tag_name, - const BT::NodeConfiguration & conf); + const BT::NodeConfig & conf); void halt() {} BT::NodeStatus tick() override; diff --git a/plansys2_executor/include/plansys2_executor/behavior_tree/check_atend_req_node.hpp b/plansys2_executor/include/plansys2_executor/behavior_tree/check_atend_req_node.hpp index fb8a25407..7ff6d2009 100644 --- a/plansys2_executor/include/plansys2_executor/behavior_tree/check_atend_req_node.hpp +++ b/plansys2_executor/include/plansys2_executor/behavior_tree/check_atend_req_node.hpp @@ -19,7 +19,7 @@ #include #include -#include "behaviortree_cpp_v3/action_node.h" +#include "behaviortree_cpp/action_node.h" #include "plansys2_problem_expert/ProblemExpertClient.hpp" #include "plansys2_executor/ActionExecutor.hpp" @@ -35,7 +35,7 @@ class CheckAtEndReq : public BT::ActionNodeBase public: CheckAtEndReq( const std::string & xml_tag_name, - const BT::NodeConfiguration & conf); + const BT::NodeConfig & conf); void halt() {} BT::NodeStatus tick() override; diff --git a/plansys2_executor/include/plansys2_executor/behavior_tree/check_overall_req_node.hpp b/plansys2_executor/include/plansys2_executor/behavior_tree/check_overall_req_node.hpp index bcb1cfdfd..a118a0bcf 100644 --- a/plansys2_executor/include/plansys2_executor/behavior_tree/check_overall_req_node.hpp +++ b/plansys2_executor/include/plansys2_executor/behavior_tree/check_overall_req_node.hpp @@ -19,7 +19,7 @@ #include #include -#include "behaviortree_cpp_v3/action_node.h" +#include "behaviortree_cpp/action_node.h" #include "plansys2_problem_expert/ProblemExpertClient.hpp" #include "plansys2_executor/ActionExecutor.hpp" @@ -35,7 +35,7 @@ class CheckOverAllReq : public BT::ActionNodeBase public: CheckOverAllReq( const std::string & xml_tag_name, - const BT::NodeConfiguration & conf); + const BT::NodeConfig & conf); void halt() {} BT::NodeStatus tick() override; diff --git a/plansys2_executor/include/plansys2_executor/behavior_tree/check_timeout_node.hpp b/plansys2_executor/include/plansys2_executor/behavior_tree/check_timeout_node.hpp index 3c51857ec..87fb0943e 100644 --- a/plansys2_executor/include/plansys2_executor/behavior_tree/check_timeout_node.hpp +++ b/plansys2_executor/include/plansys2_executor/behavior_tree/check_timeout_node.hpp @@ -19,7 +19,7 @@ #include #include -#include "behaviortree_cpp_v3/action_node.h" +#include "behaviortree_cpp/action_node.h" #include "plansys2_problem_expert/ProblemExpertClient.hpp" #include "plansys2_executor/ActionExecutor.hpp" @@ -35,7 +35,7 @@ class CheckTimeout : public BT::ActionNodeBase public: CheckTimeout( const std::string & xml_tag_name, - const BT::NodeConfiguration & conf); + const BT::NodeConfig & conf); void halt() {} BT::NodeStatus tick() override; @@ -49,7 +49,8 @@ class CheckTimeout : public BT::ActionNodeBase } private: - std::chrono::high_resolution_clock::time_point start_; + rclcpp_lifecycle::LifecycleNode::SharedPtr node_; + rclcpp::Time start_; std::shared_ptr> action_map_; std::shared_ptr problem_client_; }; diff --git a/plansys2_executor/include/plansys2_executor/behavior_tree/execute_action_node.hpp b/plansys2_executor/include/plansys2_executor/behavior_tree/execute_action_node.hpp index 6c51f3dce..6f79f24b8 100644 --- a/plansys2_executor/include/plansys2_executor/behavior_tree/execute_action_node.hpp +++ b/plansys2_executor/include/plansys2_executor/behavior_tree/execute_action_node.hpp @@ -20,7 +20,7 @@ #include #include -#include "behaviortree_cpp_v3/action_node.h" +#include "behaviortree_cpp/action_node.h" #include "plansys2_executor/ActionExecutor.hpp" @@ -35,7 +35,7 @@ class ExecuteAction : public BT::ActionNodeBase public: ExecuteAction( const std::string & xml_tag_name, - const BT::NodeConfiguration & conf); + const BT::NodeConfig & conf); void halt(); BT::NodeStatus tick() override; diff --git a/plansys2_executor/include/plansys2_executor/behavior_tree/wait_action_node.hpp b/plansys2_executor/include/plansys2_executor/behavior_tree/wait_action_node.hpp index 1c07cc9b4..9288e1713 100644 --- a/plansys2_executor/include/plansys2_executor/behavior_tree/wait_action_node.hpp +++ b/plansys2_executor/include/plansys2_executor/behavior_tree/wait_action_node.hpp @@ -20,7 +20,7 @@ #include -#include "behaviortree_cpp_v3/action_node.h" +#include "behaviortree_cpp/action_node.h" #include "plansys2_executor/ActionExecutor.hpp" @@ -34,7 +34,7 @@ class WaitAction : public BT::ActionNodeBase public: WaitAction( const std::string & xml_tag_name, - const BT::NodeConfiguration & conf); + const BT::NodeConfig & conf); void halt() {} BT::NodeStatus tick() override; diff --git a/plansys2_executor/include/plansys2_executor/behavior_tree/wait_atstart_req_node.hpp b/plansys2_executor/include/plansys2_executor/behavior_tree/wait_atstart_req_node.hpp index 5308e306c..d1b77cef0 100644 --- a/plansys2_executor/include/plansys2_executor/behavior_tree/wait_atstart_req_node.hpp +++ b/plansys2_executor/include/plansys2_executor/behavior_tree/wait_atstart_req_node.hpp @@ -19,7 +19,7 @@ #include #include -#include "behaviortree_cpp_v3/action_node.h" +#include "behaviortree_cpp/action_node.h" #include "plansys2_problem_expert/ProblemExpertClient.hpp" #include "plansys2_executor/ActionExecutor.hpp" @@ -35,7 +35,7 @@ class WaitAtStartReq : public BT::ActionNodeBase public: WaitAtStartReq( const std::string & xml_tag_name, - const BT::NodeConfiguration & conf); + const BT::NodeConfig & conf); void halt() {} BT::NodeStatus tick() override; diff --git a/plansys2_executor/package.xml b/plansys2_executor/package.xml index 1acad37a7..e6ec80d3e 100644 --- a/plansys2_executor/package.xml +++ b/plansys2_executor/package.xml @@ -28,11 +28,10 @@ plansys2_problem_expert plansys2_planner pluginlib - behaviortree_cpp_v3 + behaviortree_cpp std_msgs std_srvs - libzmq3-dev - + popf ament_lint_common diff --git a/plansys2_executor/src/plansys2_executor/ComputeBT.cpp b/plansys2_executor/src/plansys2_executor/ComputeBT.cpp index 59a4eb882..0b06a6f0b 100644 --- a/plansys2_executor/src/plansys2_executor/ComputeBT.cpp +++ b/plansys2_executor/src/plansys2_executor/ComputeBT.cpp @@ -26,14 +26,10 @@ #include "plansys2_executor/ComputeBT.hpp" -#include "behaviortree_cpp_v3/behavior_tree.h" -#include "behaviortree_cpp_v3/bt_factory.h" -#include "behaviortree_cpp_v3/utils/shared_library.h" -#include "behaviortree_cpp_v3/blackboard.h" - -#ifdef ZMQ_FOUND -#include -#endif +#include "behaviortree_cpp/behavior_tree.h" +#include "behaviortree_cpp/bt_factory.h" +#include "behaviortree_cpp/utils/shared_library.h" +#include "behaviortree_cpp/blackboard.h" #include "plansys2_executor/behavior_tree/execute_action_node.hpp" #include "plansys2_executor/behavior_tree/wait_action_node.hpp" @@ -72,13 +68,6 @@ ComputeBT::ComputeBT() 0.0); } -#ifdef ZMQ_FOUND - this->declare_parameter("enable_groot_monitoring", true); - this->declare_parameter("publisher_port", 2666); - this->declare_parameter("server_port", 2667); - this->declare_parameter("max_msgs_per_second", 25); -#endif - compute_bt_srv_ = create_service( "compute_bt", std::bind( @@ -349,28 +338,6 @@ ComputeBT::computeBTCallback( auto tree = factory.createTreeFromText(bt_xml_tree, blackboard); -#ifdef ZMQ_FOUND - unsigned int publisher_port = this->get_parameter("publisher_port").as_int(); - unsigned int server_port = this->get_parameter("server_port").as_int(); - unsigned int max_msgs_per_second = this->get_parameter("max_msgs_per_second").as_int(); - - std::unique_ptr publisher_zmq; - if (this->get_parameter("enable_groot_monitoring").as_bool()) { - RCLCPP_DEBUG( - get_logger(), - "[%s] Groot monitoring: Publisher port: %d, Server port: %d, Max msgs per second: %d", - get_name(), publisher_port, server_port, max_msgs_per_second); - try { - publisher_zmq.reset( - new BT::PublisherZMQ( - tree, max_msgs_per_second, publisher_port, - server_port)); - } catch (const BT::LogicError & exc) { - RCLCPP_ERROR(get_logger(), "ZMQ error: %s", exc.what()); - } - } -#endif - finish = true; t.join(); diff --git a/plansys2_executor/src/plansys2_executor/ExecutorNode.cpp b/plansys2_executor/src/plansys2_executor/ExecutorNode.cpp index f7faa952b..3acd5b02f 100644 --- a/plansys2_executor/src/plansys2_executor/ExecutorNode.cpp +++ b/plansys2_executor/src/plansys2_executor/ExecutorNode.cpp @@ -35,14 +35,10 @@ #include "ament_index_cpp/get_package_share_directory.hpp" -#include "behaviortree_cpp_v3/behavior_tree.h" -#include "behaviortree_cpp_v3/bt_factory.h" -#include "behaviortree_cpp_v3/utils/shared_library.h" -#include "behaviortree_cpp_v3/blackboard.h" - -#ifdef ZMQ_FOUND -#include -#endif +#include "behaviortree_cpp/behavior_tree.h" +#include "behaviortree_cpp/bt_factory.h" +#include "behaviortree_cpp/utils/shared_library.h" +#include "behaviortree_cpp/blackboard.h" #include "plansys2_executor/behavior_tree/execute_action_node.hpp" #include "plansys2_executor/behavior_tree/wait_action_node.hpp" @@ -82,13 +78,6 @@ ExecutorNode::ExecutorNode() 0.0); } -#ifdef ZMQ_FOUND - this->declare_parameter("enable_groot_monitoring", true); - this->declare_parameter("publisher_port", 2666); - this->declare_parameter("server_port", 2667); - this->declare_parameter("max_msgs_per_second", 25); -#endif - execute_plan_action_server_ = rclcpp_action::create_server( this->get_node_base_interface(), this->get_node_clock_interface(), @@ -467,28 +456,6 @@ ExecutorNode::execute(const std::shared_ptr goal_handle) auto tree = factory.createTreeFromText(bt_xml_tree, blackboard); -#ifdef ZMQ_FOUND - unsigned int publisher_port = this->get_parameter("publisher_port").as_int(); - unsigned int server_port = this->get_parameter("server_port").as_int(); - unsigned int max_msgs_per_second = this->get_parameter("max_msgs_per_second").as_int(); - - std::unique_ptr publisher_zmq; - if (this->get_parameter("enable_groot_monitoring").as_bool()) { - RCLCPP_DEBUG( - get_logger(), - "[%s] Groot monitoring: Publisher port: %d, Server port: %d, Max msgs per second: %d", - get_name(), publisher_port, server_port, max_msgs_per_second); - try { - publisher_zmq.reset( - new BT::PublisherZMQ( - tree, max_msgs_per_second, publisher_port, - server_port)); - } catch (const BT::LogicError & exc) { - RCLCPP_ERROR(get_logger(), "ZMQ error: %s", exc.what()); - } - } -#endif - auto info_pub = create_wall_timer( 1s, [this, &action_map]() { auto msgs = get_feedback_info(action_map); @@ -502,7 +469,7 @@ ExecutorNode::execute(const std::shared_ptr goal_handle) while (status == BT::NodeStatus::RUNNING && !cancel_plan_requested_) { try { - status = tree.tickRoot(); + status = tree.tickOnce(); } catch (std::exception & e) { std::cerr << e.what() << std::endl; status = BT::NodeStatus::FAILURE; diff --git a/plansys2_executor/src/plansys2_executor/behavior_tree/apply_atend_effect_node.cpp b/plansys2_executor/src/plansys2_executor/behavior_tree/apply_atend_effect_node.cpp index 16d6b00cd..6cb2e59d9 100644 --- a/plansys2_executor/src/plansys2_executor/behavior_tree/apply_atend_effect_node.cpp +++ b/plansys2_executor/src/plansys2_executor/behavior_tree/apply_atend_effect_node.cpp @@ -23,7 +23,7 @@ namespace plansys2 ApplyAtEndEffect::ApplyAtEndEffect( const std::string & xml_tag_name, - const BT::NodeConfiguration & conf) + const BT::NodeConfig & conf) : ActionNodeBase(xml_tag_name, conf) { action_map_ = diff --git a/plansys2_executor/src/plansys2_executor/behavior_tree/apply_atstart_effect_node.cpp b/plansys2_executor/src/plansys2_executor/behavior_tree/apply_atstart_effect_node.cpp index 7e6662c37..4e57977f6 100644 --- a/plansys2_executor/src/plansys2_executor/behavior_tree/apply_atstart_effect_node.cpp +++ b/plansys2_executor/src/plansys2_executor/behavior_tree/apply_atstart_effect_node.cpp @@ -23,7 +23,7 @@ namespace plansys2 ApplyAtStartEffect::ApplyAtStartEffect( const std::string & xml_tag_name, - const BT::NodeConfiguration & conf) + const BT::NodeConfig & conf) : ActionNodeBase(xml_tag_name, conf) { action_map_ = diff --git a/plansys2_executor/src/plansys2_executor/behavior_tree/check_action_node.cpp b/plansys2_executor/src/plansys2_executor/behavior_tree/check_action_node.cpp index 033ab5ba5..e429c11c4 100644 --- a/plansys2_executor/src/plansys2_executor/behavior_tree/check_action_node.cpp +++ b/plansys2_executor/src/plansys2_executor/behavior_tree/check_action_node.cpp @@ -24,7 +24,7 @@ namespace plansys2 CheckAction::CheckAction( const std::string & xml_tag_name, - const BT::NodeConfiguration & conf) + const BT::NodeConfig & conf) : ActionNodeBase(xml_tag_name, conf) { action_map_ = diff --git a/plansys2_executor/src/plansys2_executor/behavior_tree/check_atend_req_node.cpp b/plansys2_executor/src/plansys2_executor/behavior_tree/check_atend_req_node.cpp index d7c935eff..df013112f 100644 --- a/plansys2_executor/src/plansys2_executor/behavior_tree/check_atend_req_node.cpp +++ b/plansys2_executor/src/plansys2_executor/behavior_tree/check_atend_req_node.cpp @@ -24,7 +24,7 @@ namespace plansys2 CheckAtEndReq::CheckAtEndReq( const std::string & xml_tag_name, - const BT::NodeConfiguration & conf) + const BT::NodeConfig & conf) : ActionNodeBase(xml_tag_name, conf) { action_map_ = diff --git a/plansys2_executor/src/plansys2_executor/behavior_tree/check_overall_req_node.cpp b/plansys2_executor/src/plansys2_executor/behavior_tree/check_overall_req_node.cpp index 5b7cb2236..227ac6dc8 100644 --- a/plansys2_executor/src/plansys2_executor/behavior_tree/check_overall_req_node.cpp +++ b/plansys2_executor/src/plansys2_executor/behavior_tree/check_overall_req_node.cpp @@ -24,7 +24,7 @@ namespace plansys2 CheckOverAllReq::CheckOverAllReq( const std::string & xml_tag_name, - const BT::NodeConfiguration & conf) + const BT::NodeConfig & conf) : ActionNodeBase(xml_tag_name, conf) { action_map_ = diff --git a/plansys2_executor/src/plansys2_executor/behavior_tree/check_timeout_node.cpp b/plansys2_executor/src/plansys2_executor/behavior_tree/check_timeout_node.cpp index b49a6cca2..0aff9fdef 100644 --- a/plansys2_executor/src/plansys2_executor/behavior_tree/check_timeout_node.cpp +++ b/plansys2_executor/src/plansys2_executor/behavior_tree/check_timeout_node.cpp @@ -26,7 +26,7 @@ namespace plansys2 CheckTimeout::CheckTimeout( const std::string & xml_tag_name, - const BT::NodeConfiguration & conf) + const BT::NodeConfig & conf) : ActionNodeBase(xml_tag_name, conf) { action_map_ = @@ -36,6 +36,9 @@ CheckTimeout::CheckTimeout( problem_client_ = config().blackboard->get>( "problem_client"); + + node_ = config().blackboard->get("node"); + start_ = node_->now(); } BT::NodeStatus @@ -44,22 +47,18 @@ CheckTimeout::tick() std::string action; getInput("action", action); - if (status() == BT::NodeStatus::IDLE) { - start_ = std::chrono::high_resolution_clock::now(); - } - setStatus(BT::NodeStatus::RUNNING); - if ((*action_map_)[action].action_executor != nullptr) { double duration = (*action_map_)[action].duration; double duration_overrun_percentage = (*action_map_)[action].duration_overrun_percentage; if (duration_overrun_percentage >= 0) { double max_duration = (1.0 + duration_overrun_percentage / 100.0) * duration; - auto current_time = std::chrono::high_resolution_clock::now(); - auto elapsed_time = std::chrono::duration_cast( - current_time - start_); - if (elapsed_time > std::chrono::duration(max_duration)) { - std::cerr << "Actual duration of " << action << " exceeds max duration (" << std::fixed << - std::setprecision(2) << max_duration << " secs)." << std::endl; + auto current_time = node_->now(); + auto elapsed_time = (current_time - start_).seconds(); + if (elapsed_time > max_duration) { + RCLCPP_ERROR_STREAM( + node_->get_logger(), + "Actual duration of " << action << " exceeds max duration (" << std::fixed << + std::setprecision(2) << max_duration << " secs)."); return BT::NodeStatus::FAILURE; } } diff --git a/plansys2_executor/src/plansys2_executor/behavior_tree/execute_action_node.cpp b/plansys2_executor/src/plansys2_executor/behavior_tree/execute_action_node.cpp index 6a6cdb3d2..61214ce9d 100644 --- a/plansys2_executor/src/plansys2_executor/behavior_tree/execute_action_node.cpp +++ b/plansys2_executor/src/plansys2_executor/behavior_tree/execute_action_node.cpp @@ -23,7 +23,7 @@ namespace plansys2 ExecuteAction::ExecuteAction( const std::string & xml_tag_name, - const BT::NodeConfiguration & conf) + const BT::NodeConfig & conf) : ActionNodeBase(xml_tag_name, conf) { action_map_ = diff --git a/plansys2_executor/src/plansys2_executor/behavior_tree/wait_action_node.cpp b/plansys2_executor/src/plansys2_executor/behavior_tree/wait_action_node.cpp index d4f9c5109..20a492a91 100644 --- a/plansys2_executor/src/plansys2_executor/behavior_tree/wait_action_node.cpp +++ b/plansys2_executor/src/plansys2_executor/behavior_tree/wait_action_node.cpp @@ -23,7 +23,7 @@ namespace plansys2 WaitAction::WaitAction( const std::string & xml_tag_name, - const BT::NodeConfiguration & conf) + const BT::NodeConfig & conf) : ActionNodeBase(xml_tag_name, conf) { action_map_ = diff --git a/plansys2_executor/src/plansys2_executor/behavior_tree/wait_atstart_req_node.cpp b/plansys2_executor/src/plansys2_executor/behavior_tree/wait_atstart_req_node.cpp index 370e2caff..50c55f037 100644 --- a/plansys2_executor/src/plansys2_executor/behavior_tree/wait_atstart_req_node.cpp +++ b/plansys2_executor/src/plansys2_executor/behavior_tree/wait_atstart_req_node.cpp @@ -24,7 +24,7 @@ namespace plansys2 WaitAtStartReq::WaitAtStartReq( const std::string & xml_tag_name, - const BT::NodeConfiguration & conf) + const BT::NodeConfig & conf) : ActionNodeBase(xml_tag_name, conf) { action_map_ = diff --git a/plansys2_executor/src/plansys2_executor/bt_builder_plugins/simple_bt_builder.cpp b/plansys2_executor/src/plansys2_executor/bt_builder_plugins/simple_bt_builder.cpp index d99fb8157..2a16b89b1 100644 --- a/plansys2_executor/src/plansys2_executor/bt_builder_plugins/simple_bt_builder.cpp +++ b/plansys2_executor/src/plansys2_executor/bt_builder_plugins/simple_bt_builder.cpp @@ -447,10 +447,10 @@ SimpleBTBuilder::get_tree(const plansys2_msgs::msg::Plan & current_plan) std::list used_nodes; if (graph_->roots.size() > 1) { - bt_ = std::string("\n") + + bt_ = std::string("\n") + t(1) + "\n" + - t(2) + "roots.size()) + - "\" failure_threshold=\"1\">\n"; + t(2) + "roots.size()) + + "\" failure_count=\"1\">\n"; for (const auto & node : graph_->roots) { bt_ = bt_ + get_flow_tree(node, used_nodes, 3); @@ -459,7 +459,7 @@ SimpleBTBuilder::get_tree(const plansys2_msgs::msg::Plan & current_plan) bt_ = bt_ + t(2) + "\n" + t(1) + "\n\n"; } else { - bt_ = std::string("\n") + + bt_ = std::string("\n") + t(1) + "\n"; bt_ = bt_ + get_flow_tree(*graph_->roots.begin(), used_nodes, 2); @@ -613,8 +613,8 @@ SimpleBTBuilder::get_flow_tree( ret = ret + execution_block(node, l + 1); ret = ret + t(l + 1) + - "out_arcs.size()) + - "\" failure_threshold=\"1\">\n"; + "out_arcs.size()) + + "\" failure_count=\"1\">\n"; for (const auto & child_node : node->out_arcs) { ret = ret + get_flow_tree(child_node, used_nodes, l + 2); diff --git a/plansys2_executor/src/plansys2_executor/bt_builder_plugins/stn_bt_builder.cpp b/plansys2_executor/src/plansys2_executor/bt_builder_plugins/stn_bt_builder.cpp index f380aa181..e32f4b3a6 100644 --- a/plansys2_executor/src/plansys2_executor/bt_builder_plugins/stn_bt_builder.cpp +++ b/plansys2_executor/src/plansys2_executor/bt_builder_plugins/stn_bt_builder.cpp @@ -245,7 +245,7 @@ STNBTBuilder::build_bt(const Graph::Ptr stn) const std::set used; const auto & root = stn->nodes.front(); - auto bt = std::string("\n") + t(1) + + auto bt = std::string("\n") + t(1) + "\n"; bt = bt + get_flow(root, nullptr, used, 1); bt = bt + t(1) + "\n\n"; @@ -1161,8 +1161,8 @@ STNBTBuilder::get_flow( int n = 0; if (num_output_arcs > 1) { flow = flow + t(l + 1) + - "\n"; + "\n"; n = 1; } diff --git a/plansys2_executor/test/unit/action_execution_test.cpp b/plansys2_executor/test/unit/action_execution_test.cpp index ce86dd6c7..74d2b24bc 100644 --- a/plansys2_executor/test/unit/action_execution_test.cpp +++ b/plansys2_executor/test/unit/action_execution_test.cpp @@ -36,10 +36,10 @@ #include "plansys2_executor/ExecutorClient.hpp" #include "plansys2_problem_expert/Utils.hpp" -#include "behaviortree_cpp_v3/behavior_tree.h" -#include "behaviortree_cpp_v3/bt_factory.h" -#include "behaviortree_cpp_v3/utils/shared_library.h" -#include "behaviortree_cpp_v3/blackboard.h" +#include "behaviortree_cpp/behavior_tree.h" +#include "behaviortree_cpp/bt_factory.h" +#include "behaviortree_cpp/utils/shared_library.h" +#include "behaviortree_cpp/blackboard.h" #include "plansys2_executor/behavior_tree/execute_action_node.hpp" #include "plansys2_executor/behavior_tree/wait_action_node.hpp" diff --git a/plansys2_executor/test/unit/bt_node_test.cpp b/plansys2_executor/test/unit/bt_node_test.cpp index f1a511738..bb83211be 100644 --- a/plansys2_executor/test/unit/bt_node_test.cpp +++ b/plansys2_executor/test/unit/bt_node_test.cpp @@ -35,10 +35,10 @@ #include "plansys2_problem_expert/Utils.hpp" #include "plansys2_pddl_parser/Utils.h" -#include "behaviortree_cpp_v3/behavior_tree.h" -#include "behaviortree_cpp_v3/bt_factory.h" -#include "behaviortree_cpp_v3/utils/shared_library.h" -#include "behaviortree_cpp_v3/blackboard.h" +#include "behaviortree_cpp/behavior_tree.h" +#include "behaviortree_cpp/bt_factory.h" +#include "behaviortree_cpp/utils/shared_library.h" +#include "behaviortree_cpp/blackboard.h" #include "plansys2_executor/behavior_tree/execute_action_node.hpp" #include "plansys2_executor/behavior_tree/wait_action_node.hpp" @@ -118,7 +118,7 @@ TEST(problem_expert, wait_overall_req_test) std::string bt_xml_tree = R"( - + @@ -151,7 +151,7 @@ TEST(problem_expert, wait_overall_req_test) auto tree = factory.createTreeFromText(bt_xml_tree, blackboard); auto status = BT::NodeStatus::RUNNING; - status = tree.tickRoot(); + status = tree.tickOnce(); ASSERT_EQ(status, BT::NodeStatus::FAILURE); for (const auto & pred : predicates) { @@ -161,7 +161,7 @@ TEST(problem_expert, wait_overall_req_test) tree = factory.createTreeFromText(bt_xml_tree, blackboard); status = BT::NodeStatus::RUNNING; - status = tree.tickRoot(); + status = tree.tickOnce(); ASSERT_EQ(status, BT::NodeStatus::SUCCESS); } catch (std::exception & e) { std::cerr << e.what() << std::endl; @@ -232,7 +232,7 @@ TEST(problem_expert, wait_atstart_req_test) std::string bt_xml_tree = R"( - + @@ -265,11 +265,11 @@ TEST(problem_expert, wait_atstart_req_test) auto tree = factory.createTreeFromText(bt_xml_tree, blackboard); auto status = BT::NodeStatus::RUNNING; - status = tree.tickRoot(); + status = tree.tickOnce(); ASSERT_EQ(status, BT::NodeStatus::RUNNING); - status = tree.tickRoot(); + status = tree.tickOnce(); ASSERT_EQ(status, BT::NodeStatus::RUNNING); - status = tree.tickRoot(); + status = tree.tickOnce(); ASSERT_EQ(status, BT::NodeStatus::RUNNING); @@ -277,7 +277,7 @@ TEST(problem_expert, wait_atstart_req_test) ASSERT_TRUE(problem_client->addPredicate(plansys2::Predicate(pred))); } - status = tree.tickRoot(); + status = tree.tickOnce(); ASSERT_EQ(status, BT::NodeStatus::SUCCESS); } catch (std::exception & e) { std::cerr << e.what() << std::endl; @@ -348,7 +348,7 @@ TEST(problem_expert, wait_atend_req_test) std::string bt_xml_tree = R"( - + @@ -381,7 +381,7 @@ TEST(problem_expert, wait_atend_req_test) auto tree = factory.createTreeFromText(bt_xml_tree, blackboard); auto status = BT::NodeStatus::RUNNING; - status = tree.tickRoot(); + status = tree.tickOnce(); ASSERT_EQ(status, BT::NodeStatus::FAILURE); for (const auto & pred : predicates) { @@ -391,7 +391,7 @@ TEST(problem_expert, wait_atend_req_test) tree = factory.createTreeFromText(bt_xml_tree, blackboard); status = BT::NodeStatus::RUNNING; - status = tree.tickRoot(); + status = tree.tickOnce(); ASSERT_EQ(status, BT::NodeStatus::SUCCESS); } catch (std::exception & e) { std::cerr << e.what() << std::endl; @@ -462,7 +462,7 @@ TEST(problem_expert, at_start_effect_test) std::string bt_xml_tree = R"( - + @@ -498,7 +498,7 @@ TEST(problem_expert, at_start_effect_test) auto tree = factory.createTreeFromText(bt_xml_tree, blackboard); auto status = BT::NodeStatus::RUNNING; - status = tree.tickRoot(); + status = tree.tickOnce(); ASSERT_EQ(status, BT::NodeStatus::SUCCESS); { @@ -581,7 +581,7 @@ TEST(problem_expert, at_end_effect_test) std::string bt_xml_tree = R"( - + @@ -616,7 +616,7 @@ TEST(problem_expert, at_end_effect_test) auto tree = factory.createTreeFromText(bt_xml_tree, blackboard); auto status = BT::NodeStatus::RUNNING; - status = tree.tickRoot(); + status = tree.tickOnce(); ASSERT_EQ(status, BT::NodeStatus::SUCCESS); { diff --git a/plansys2_executor/test/unit/bt_node_test_charging.cpp b/plansys2_executor/test/unit/bt_node_test_charging.cpp index 462471f00..255643e1b 100644 --- a/plansys2_executor/test/unit/bt_node_test_charging.cpp +++ b/plansys2_executor/test/unit/bt_node_test_charging.cpp @@ -34,10 +34,10 @@ #include "plansys2_executor/ActionExecutorClient.hpp" #include "plansys2_problem_expert/Utils.hpp" -#include "behaviortree_cpp_v3/behavior_tree.h" -#include "behaviortree_cpp_v3/bt_factory.h" -#include "behaviortree_cpp_v3/utils/shared_library.h" -#include "behaviortree_cpp_v3/blackboard.h" +#include "behaviortree_cpp/behavior_tree.h" +#include "behaviortree_cpp/bt_factory.h" +#include "behaviortree_cpp/utils/shared_library.h" +#include "behaviortree_cpp/blackboard.h" #include "plansys2_executor/behavior_tree/execute_action_node.hpp" #include "plansys2_executor/behavior_tree/wait_atstart_req_node.hpp" @@ -113,7 +113,7 @@ TEST(problem_expert, wait_atstart_req_test) std::string bt_xml_tree = R"( - + @@ -152,11 +152,11 @@ TEST(problem_expert, wait_atstart_req_test) auto tree = factory.createTreeFromText(bt_xml_tree, blackboard); auto status = BT::NodeStatus::RUNNING; - status = tree.tickRoot(); + status = tree.tickOnce(); ASSERT_EQ(status, BT::NodeStatus::RUNNING); - status = tree.tickRoot(); + status = tree.tickOnce(); ASSERT_EQ(status, BT::NodeStatus::RUNNING); - status = tree.tickRoot(); + status = tree.tickOnce(); ASSERT_EQ(status, BT::NodeStatus::RUNNING); for (const auto & pred : predicates) { @@ -167,7 +167,7 @@ TEST(problem_expert, wait_atstart_req_test) ASSERT_TRUE(problem_client->addFunction(plansys2::Function(func))); } - status = tree.tickRoot(); + status = tree.tickOnce(); ASSERT_EQ(status, BT::NodeStatus::SUCCESS); } catch (std::exception & e) { std::cerr << e.what() << std::endl; @@ -238,7 +238,7 @@ TEST(problem_expert, apply_atstart_effect_test) std::string bt_xml_tree = R"( - + @@ -285,7 +285,7 @@ TEST(problem_expert, apply_atstart_effect_test) auto tree = factory.createTreeFromText(bt_xml_tree, blackboard); auto status = BT::NodeStatus::RUNNING; - status = tree.tickRoot(); + status = tree.tickOnce(); ASSERT_EQ(status, BT::NodeStatus::SUCCESS); { @@ -365,7 +365,7 @@ TEST(problem_expert, apply_atend_effect_test) std::string bt_xml_tree = R"( - + @@ -412,7 +412,7 @@ TEST(problem_expert, apply_atend_effect_test) auto tree = factory.createTreeFromText(bt_xml_tree, blackboard); auto status = BT::NodeStatus::RUNNING; - status = tree.tickRoot(); + status = tree.tickOnce(); ASSERT_EQ(status, BT::NodeStatus::SUCCESS); { diff --git a/plansys2_executor/test/unit/executor_test.cpp b/plansys2_executor/test/unit/executor_test.cpp index c84e12f9d..e19c0b02f 100644 --- a/plansys2_executor/test/unit/executor_test.cpp +++ b/plansys2_executor/test/unit/executor_test.cpp @@ -36,10 +36,10 @@ #include "plansys2_executor/ExecutorClient.hpp" #include "plansys2_problem_expert/Utils.hpp" -#include "behaviortree_cpp_v3/behavior_tree.h" -#include "behaviortree_cpp_v3/bt_factory.h" -#include "behaviortree_cpp_v3/utils/shared_library.h" -#include "behaviortree_cpp_v3/blackboard.h" +#include "behaviortree_cpp/behavior_tree.h" +#include "behaviortree_cpp/bt_factory.h" +#include "behaviortree_cpp/utils/shared_library.h" +#include "behaviortree_cpp/blackboard.h" #include "plansys2_executor/behavior_tree/execute_action_node.hpp" #include "plansys2_executor/behavior_tree/wait_action_node.hpp" @@ -107,7 +107,7 @@ class MoveAction : public plansys2::ActionExecutorClient { std::cerr << "MoveAction::on_activate" << std::endl; counter_ = 0; - start_ = std::chrono::high_resolution_clock::now(); + start_ = now(); return ActionExecutorClient::on_activate(state); } @@ -120,17 +120,18 @@ class MoveAction : public plansys2::ActionExecutorClient } cycles_++; - auto current_time = std::chrono::high_resolution_clock::now(); - auto elapsed_time = std::chrono::duration_cast( - current_time - start_); + auto current_time = now(); + auto elapsed_time = (current_time - start_).seconds(); + + rclcpp::Rate rate(100); if (runtime_ > 1e-5) { - if (elapsed_time > std::chrono::duration(runtime_)) { + if (elapsed_time > runtime_) { finish(true, 1.0, "completed"); executions_++; } else { - send_feedback((static_cast(elapsed_time.count()) / 1000.0) / runtime_, "running"); - std::this_thread::sleep_for(std::chrono::milliseconds(10)); + send_feedback(elapsed_time / runtime_, "running"); + rate.sleep(); } } else { if (counter_++ > 3) { @@ -146,7 +147,7 @@ class MoveAction : public plansys2::ActionExecutorClient int executions_; int cycles_; double runtime_; - std::chrono::high_resolution_clock::time_point start_; + rclcpp::Time start_; }; class TransportAction : public plansys2::ActionExecutorClient @@ -248,10 +249,10 @@ TEST(executor, action_executor_client) std::string bt_xml_tree = R"( - + - + @@ -281,7 +282,7 @@ TEST(executor, action_executor_client) auto status = BT::NodeStatus::RUNNING; while (status != BT::NodeStatus::SUCCESS) { - status = tree.tickRoot(); + status = tree.tickOnce(); } ASSERT_EQ(status, BT::NodeStatus::SUCCESS); @@ -691,7 +692,7 @@ TEST(executor, action_real_action_1) std::string bt_xml_tree = R"( - + @@ -742,7 +743,7 @@ TEST(executor, action_real_action_1) auto status = BT::NodeStatus::RUNNING; for (int i = 0; i < 10; i++) { - status = tree.tickRoot(); + status = tree.tickOnce(); ASSERT_EQ(status, BT::NodeStatus::RUNNING); ASSERT_EQ(WaitActionTest::test_status, BT::NodeStatus::RUNNING); } @@ -753,7 +754,7 @@ TEST(executor, action_real_action_1) // Test ApplyAtStartEffect and CheckOverAllReq bt_xml_tree = R"( - + @@ -786,7 +787,7 @@ TEST(executor, action_real_action_1) auto status = BT::NodeStatus::RUNNING; ASSERT_TRUE(problem_client->existPredicate(plansys2::Predicate("(robot_available r2d2)"))); - status = tree.tickRoot(); + status = tree.tickOnce(); ASSERT_EQ(ApplyAtStartEffectTest::test_status, BT::NodeStatus::SUCCESS); ASSERT_FALSE(problem_client->existPredicate(plansys2::Predicate("(robot_available r2d2)"))); @@ -795,16 +796,16 @@ TEST(executor, action_real_action_1) plansys2::Predicate( "(robot_at r2d2 steering_wheels_zone)"))); - status = tree.tickRoot(); + status = tree.tickOnce(); ASSERT_EQ(CheckOverAllReqTest::test_status, BT::NodeStatus::SUCCESS); ASSERT_EQ(ExecuteActionTest::test_status, BT::NodeStatus::RUNNING); - status = tree.tickRoot(); + status = tree.tickOnce(); ASSERT_EQ(CheckOverAllReqTest::test_status, BT::NodeStatus::SUCCESS); ASSERT_EQ(ExecuteActionTest::test_status, BT::NodeStatus::RUNNING); ASSERT_TRUE(problem_client->removePredicate(plansys2::Predicate("(battery_full r2d2)"))); - status = tree.tickRoot(); + status = tree.tickOnce(); ASSERT_EQ(CheckOverAllReqTest::test_status, BT::NodeStatus::FAILURE); ASSERT_EQ(status, BT::NodeStatus::FAILURE); } catch (const std::exception & e) { @@ -829,7 +830,7 @@ TEST(executor, action_real_action_1) ASSERT_TRUE(problem_client->existPredicate(plansys2::Predicate("(robot_available r2d2)"))); while (ApplyAtStartEffectTest::test_status != BT::NodeStatus::SUCCESS) { - status = tree.tickRoot(); + status = tree.tickOnce(); } ASSERT_FALSE( @@ -839,12 +840,12 @@ TEST(executor, action_real_action_1) ASSERT_FALSE(problem_client->existPredicate(plansys2::Predicate("(robot_available r2d2)"))); while (ExecuteActionTest::test_status != BT::NodeStatus::SUCCESS) { - status = tree.tickRoot(); + status = tree.tickOnce(); ASSERT_EQ(CheckOverAllReqTest::test_status, BT::NodeStatus::SUCCESS); } while (ApplyAtEndEffectTest::test_status != BT::NodeStatus::SUCCESS) { - status = tree.tickRoot(); + status = tree.tickOnce(); } ASSERT_TRUE( @@ -946,7 +947,7 @@ TEST(executor, cancel_bt_execution) std::string bt_xml_tree = R"( - + @@ -1005,7 +1006,7 @@ TEST(executor, cancel_bt_execution) auto status = BT::NodeStatus::RUNNING; ASSERT_TRUE(problem_client->existPredicate(plansys2::Predicate("(robot_available r2d2)"))); - status = tree.tickRoot(); + status = tree.tickOnce(); ASSERT_EQ(ApplyAtStartEffectTest::test_status, BT::NodeStatus::SUCCESS); ASSERT_FALSE(problem_client->existPredicate(plansys2::Predicate("(robot_available r2d2)"))); @@ -1014,10 +1015,10 @@ TEST(executor, cancel_bt_execution) plansys2::Predicate( "(robot_at r2d2 steering_wheels_zone)"))); - status = tree.tickRoot(); + status = tree.tickOnce(); ASSERT_EQ(CheckOverAllReqTest::test_status, BT::NodeStatus::SUCCESS); ASSERT_EQ(ExecuteActionTest::test_status, BT::NodeStatus::RUNNING); - status = tree.tickRoot(); + status = tree.tickOnce(); ASSERT_EQ(CheckOverAllReqTest::test_status, BT::NodeStatus::SUCCESS); ASSERT_EQ(ExecuteActionTest::test_status, BT::NodeStatus::RUNNING); @@ -1037,9 +1038,9 @@ TEST(executor, cancel_bt_execution) tree = factory.createTreeFromText(bt_xml_tree, blackboard); - status = tree.tickRoot(); - status = tree.tickRoot(); - status = tree.tickRoot(); + status = tree.tickOnce(); + status = tree.tickOnce(); + status = tree.tickOnce(); ASSERT_EQ(ApplyAtStartEffectTest::test_status, BT::NodeStatus::SUCCESS); ASSERT_EQ(CheckOverAllReqTest::test_status, BT::NodeStatus::SUCCESS); ASSERT_EQ(ExecuteActionTest::test_status, BT::NodeStatus::RUNNING); @@ -1550,7 +1551,6 @@ TEST(executor, executor_client_cancel_plan) t.join(); } - TEST(executor, action_timeout) { auto test_node_1 = rclcpp::Node::make_shared("test_node_1"); diff --git a/plansys2_executor/test/unit/simple_btbuilder_tests.cpp b/plansys2_executor/test/unit/simple_btbuilder_tests.cpp index 098eae7f3..b6e61cacc 100644 --- a/plansys2_executor/test/unit/simple_btbuilder_tests.cpp +++ b/plansys2_executor/test/unit/simple_btbuilder_tests.cpp @@ -39,10 +39,10 @@ #include "plansys2_executor/ExecutorNode.hpp" #include "plansys2_executor/ExecutorClient.hpp" -#include "behaviortree_cpp_v3/behavior_tree.h" -#include "behaviortree_cpp_v3/bt_factory.h" -#include "behaviortree_cpp_v3/utils/shared_library.h" -#include "behaviortree_cpp_v3/blackboard.h" +#include "behaviortree_cpp/behavior_tree.h" +#include "behaviortree_cpp/bt_factory.h" +#include "behaviortree_cpp/utils/shared_library.h" +#include "behaviortree_cpp/blackboard.h" #include "plansys2_executor/behavior_tree/execute_action_node.hpp" #include "plansys2_executor/behavior_tree/wait_action_node.hpp" diff --git a/plansys2_tests/CMakeLists.txt b/plansys2_tests/CMakeLists.txt index 4073ed13c..2959a571e 100644 --- a/plansys2_tests/CMakeLists.txt +++ b/plansys2_tests/CMakeLists.txt @@ -15,7 +15,7 @@ find_package(plansys2_domain_expert REQUIRED) find_package(plansys2_problem_expert REQUIRED) find_package(plansys2_planner REQUIRED) find_package(plansys2_executor REQUIRED) -find_package(behaviortree_cpp_v3 REQUIRED) +find_package(behaviortree_cpp REQUIRED) set(CMAKE_CXX_STANDARD 17) @@ -32,7 +32,7 @@ set(dependencies plansys2_problem_expert plansys2_planner plansys2_executor - behaviortree_cpp_v3 + behaviortree_cpp ) include_directories(include) diff --git a/plansys2_tests/package.xml b/plansys2_tests/package.xml index 5616549ee..0e540a9ca 100644 --- a/plansys2_tests/package.xml +++ b/plansys2_tests/package.xml @@ -24,7 +24,7 @@ plansys2_problem_expert plansys2_planner plansys2_executor - behaviortree_cpp_v3 + behaviortree_cpp popf