From 7fd2df91c896c5190748874a2291cf92baae1ea0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Victor=20Massagu=C3=A9=20Respall?= Date: Tue, 29 Nov 2022 09:28:39 +0100 Subject: [PATCH 1/8] Migrate nav2 wrapper --- CMakeLists.txt | 84 +-- .../behavior_tree_engine.hpp | 100 +++ include/behaviortree_ros2/bt_action_node.hpp | 635 +++++++++++------- include/behaviortree_ros2/bt_conversions.hpp | 192 ++++++ include/behaviortree_ros2/bt_service_node.hpp | 241 +++++++ package.xml | 33 +- src/behavior_tree_engine.cpp | 106 +++ test/CMakeLists.txt | 7 + test/sleep_server.cpp | 103 --- test/test_action_server.hpp | 100 +++ test/test_behavior_tree_fixture.hpp | 93 +++ test/test_bt_conversions.cpp | 269 ++++++++ test/test_client.cpp | 116 ---- test/test_dummy_tree_node.hpp | 59 ++ test/test_service.hpp | 60 ++ test/test_transform_handler.hpp | 165 +++++ 16 files changed, 1828 insertions(+), 535 deletions(-) create mode 100644 include/behaviortree_ros2/behavior_tree_engine.hpp create mode 100644 include/behaviortree_ros2/bt_conversions.hpp create mode 100644 include/behaviortree_ros2/bt_service_node.hpp create mode 100644 src/behavior_tree_engine.cpp create mode 100644 test/CMakeLists.txt delete mode 100644 test/sleep_server.cpp create mode 100644 test/test_action_server.hpp create mode 100644 test/test_behavior_tree_fixture.hpp create mode 100644 test/test_bt_conversions.cpp delete mode 100644 test/test_client.cpp create mode 100644 test/test_dummy_tree_node.hpp create mode 100644 test/test_service.hpp create mode 100644 test/test_transform_handler.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 0412a85..6a0df0d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.16) +cmake_minimum_required(VERSION 3.5) project(behaviortree_ros2) set(CMAKE_CXX_STANDARD 17) @@ -7,74 +7,54 @@ set(CMAKE_POSITION_INDEPENDENT_CODE ON) ###################################################### -set(THIS_PACKAGE_INCLUDE_DEPENDS - rclcpp - rclcpp_action - ament_index_cpp - behaviortree_cpp_v3) - find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED ) find_package(rclcpp_action REQUIRED ) -find_package(behaviortree_cpp_v3 REQUIRED ) +find_package(behaviortree_cpp REQUIRED ) find_package(ament_index_cpp REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(Boost COMPONENTS coroutine REQUIRED) -find_package(rosidl_default_generators REQUIRED) +set(THIS_PACKAGE_INCLUDE_DEPENDS + rclcpp + rclcpp_action + ament_index_cpp + behaviortree_cpp + geometry_msgs + std_msgs + Boost) -rosidl_generate_interfaces(${PROJECT_NAME} - "action/Sleep.action" +include_directories( + include ) - -###################################################### - -###################################################### -# TESTS - -add_executable(sleep_server test/sleep_server.cpp) -target_include_directories(sleep_server PRIVATE - $ - $) -ament_target_dependencies(sleep_server - ${THIS_PACKAGE_INCLUDE_DEPENDS} +add_library(${PROJECT_NAME} SHARED + src/behavior_tree_engine.cpp ) -rosidl_target_interfaces(sleep_server ${PROJECT_NAME} "rosidl_typesupport_cpp") - - -add_executable(test_client test/test_client.cpp) -target_include_directories(test_client PRIVATE - $ - $) -ament_target_dependencies(test_client +ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS} ) -rosidl_target_interfaces(test_client ${PROJECT_NAME} "rosidl_typesupport_cpp") - - -#add_executable(test_server test/test_server.cpp) -#add_dependencies(test_server ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -#target_link_libraries(test_server ${catkin_LIBRARIES} ) -###################################################### -# INSTALL - -install(TARGETS - sleep_server - test_client - RUNTIME DESTINATION bin +install(TARGETS + ${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib - ) - -install( - DIRECTORY include/ - DESTINATION include + RUNTIME DESTINATION bin ) -###################################################### +install(DIRECTORY include/ + DESTINATION include/ +) -ament_export_include_directories(include) +ament_export_include_directories( + include +) -ament_package() +ament_export_libraries( + ${PROJECT_NAME} +) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/include/behaviortree_ros2/behavior_tree_engine.hpp b/include/behaviortree_ros2/behavior_tree_engine.hpp new file mode 100644 index 0000000..53da8f8 --- /dev/null +++ b/include/behaviortree_ros2/behavior_tree_engine.hpp @@ -0,0 +1,100 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Florian Gramss +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BEHAVIOR_TREE_ROS2__BEHAVIOR_TREE_ENGINE_HPP_ +#define BEHAVIOR_TREE_ROS2__BEHAVIOR_TREE_ENGINE_HPP_ + +#include +#include +#include + +#include "behaviortree_cpp/behavior_tree.h" +#include "behaviortree_cpp/bt_factory.h" +#include "behaviortree_cpp/xml_parsing.h" +#include "behaviortree_cpp/loggers/bt_zmq_publisher.h" + + +namespace BT +{ + +/** + * @enum nav2_behavior_tree::BtStatus + * @brief An enum class representing BT execution status + */ +enum class BtStatus { SUCCEEDED, FAILED, CANCELED }; + +/** + * @class nav2_behavior_tree::BehaviorTreeEngine + * @brief A class to create and handle behavior trees + */ +class BehaviorTreeEngine +{ +public: + /** + * @brief A constructor for nav2_behavior_tree::BehaviorTreeEngine + * @param plugin_libraries vector of BT plugin library names to load + */ + explicit BehaviorTreeEngine(const std::vector & plugin_libraries); + virtual ~BehaviorTreeEngine() {} + + /** + * @brief Function to execute a BT at a specific rate + * @param tree BT to execute + * @param onLoop Function to execute on each iteration of BT execution + * @param cancelRequested Function to check if cancel was requested during BT execution + * @param loopTimeout Time period for each iteration of BT execution + * @return nav2_behavior_tree::BtStatus Status of BT execution + */ + BtStatus run( + Tree * tree, + std::function onLoop, + std::function cancelRequested, + std::chrono::milliseconds loopTimeout = std::chrono::milliseconds(10)); + + /** + * @brief Function to create a BT from a XML string + * @param xml_string XML string representing BT + * @param blackboard Blackboard for BT + * @return Tree Created behavior tree + */ + Tree createTreeFromText( + const std::string & xml_string, + Blackboard::Ptr blackboard); + + /** + * @brief Function to create a BT from an XML file + * @param file_path Path to BT XML file + * @param blackboard Blackboard for BT + * @return Tree Created behavior tree + */ + Tree createTreeFromFile( + const std::string & file_path, + Blackboard::Ptr blackboard); + + /** + * @brief Function to explicitly reset all BT nodes to initial state + * @param root_node Pointer to BT root node + */ + void haltAllActions(TreeNode * root_node); + +protected: + // The factory that will be used to dynamically construct the behavior tree + BehaviorTreeFactory factory_; + std::unique_ptr groot_monitor_; +}; + +} // namespace BT + +#endif // BEHAVIOR_TREE_ROS2__BEHAVIOR_TREE_ENGINE_HPP_ diff --git a/include/behaviortree_ros2/bt_action_node.hpp b/include/behaviortree_ros2/bt_action_node.hpp index 06a77f1..6e90f1d 100644 --- a/include/behaviortree_ros2/bt_action_node.hpp +++ b/include/behaviortree_ros2/bt_action_node.hpp @@ -1,333 +1,452 @@ +// Copyright (c) 2018 Intel Corporation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #ifndef BEHAVIOR_TREE_ROS2__BT_ACTION_NODE_HPP_ #define BEHAVIOR_TREE_ROS2__BT_ACTION_NODE_HPP_ #include #include -#include -#include -#include "behaviortree_cpp_v3/action_node.h" -#include "behaviortree_cpp_v3/bt_factory.h" -#include "rclcpp_action/rclcpp_action.hpp" - -namespace BT -{ +#include +#include -struct ActionNodeParams -{ - std::shared_ptr nh; - std::string action_name; - std::chrono::milliseconds server_timeout; -}; +#include "behaviortree_cpp/action_node.h" +#include "behaviortree_ros2/bt_conversions.hpp" -enum ActionNodeErrorCode +namespace BT { - SERVER_UNREACHABLE, - SEND_GOAL_TIMEOUT, - GOAL_REJECTED_BY_SERVER, - ACTION_ABORTED, - ACTION_CANCELLED, - INVALID_GOAL -}; /** - * @brief Abstract class representing use to call a ROS2 Action (client). - * - * It will try to be non-blocking for the entire duration of the call. - * The derived class whould reimplement the virtual methods as described below. + * @brief Abstract class representing an action based BT node + * @tparam ActionT Type of action */ template -class RosActionNode : public BT::ActionNodeBase +class BtActionNode : public ActionNodeBase { - public: - // Type definitions - using ActionType = ActionT; - using ActionClient = typename rclcpp_action::Client; - using Goal = typename ActionT::Goal; - using GoalHandle = typename rclcpp_action::ClientGoalHandle; - using WrappedResult = typename rclcpp_action::ClientGoalHandle::WrappedResult; - using Feedback = typename ActionT::Feedback; - using Params = ActionNodeParams; - - /** You are not supposed to instantiate this class directly, the factory will do it. - * To register this class into the factory, use: - * - * RegisterRosAction(factory, params) - * - * Note that if the external_action_client is not set, the constructor will build its own. - * */ - explicit RosActionNode(const std::string & instance_name, - const BT::NodeConfiguration& conf, - const ActionNodeParams& params, - typename std::shared_ptr external_action_client = {}); - - virtual ~RosActionNode() = default; - - NodeStatus tick() override final; - - /// The default halt() implementation will call cancelGoal is necessary. - void halt() override; - - /** setGoal is a callback invoked to return the goal message (ActionT::Goal). - * If conditions are not met, it should return "false" and the BT::Action - * will return FAILURE. + /** + * @brief A nav2_behavior_tree::BtActionNode constructor + * @param xml_tag_name Name for the XML tag for this node + * @param action_name Action name this node creates a client for + * @param conf BT node configuration */ - virtual bool setGoal(Goal& goal) = 0; + BtActionNode( + const std::string & xml_tag_name, + const std::string & action_name, + const NodeConfig & conf) + : ActionNodeBase(xml_tag_name, conf), action_name_(action_name) + { + node_ = config().blackboard->template get("node"); + callback_group_ = node_->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, + false); + callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); + + // Get the required items from the blackboard + bt_loop_duration_ = + config().blackboard->template get("bt_loop_duration"); + server_timeout_ = + config().blackboard->template get("server_timeout"); + getInput("server_timeout", server_timeout_); + + // Get the required items from the blackboard + server_timeout_ = + config().blackboard->template get("server_timeout"); + getInput("server_timeout", server_timeout_); + // Initialize the input and output messages + goal_ = typename ActionT::Goal(); + result_ = typename rclcpp_action::ClientGoalHandle::WrappedResult(); + + std::string remapped_action_name; + if (getInput("server_name", remapped_action_name)) { + action_name_ = remapped_action_name; + } + std::string node_namespace; + node_namespace = node_->get_namespace(); + // Append namespace to the action name + if(node_namespace != "/") { + action_name_ = node_namespace + "/" + action_name_; + } + createActionClient(action_name_); - /** Callback invoked when the result is received by the server. - * It is up to the user to define if the action returns SUCCESS or FAILURE. - */ - virtual BT::NodeStatus onResultReceived(const WrappedResult& result) = 0; + // Give the derive class a chance to do any initialization + RCLCPP_DEBUG(node_->get_logger(), "\"%s\" BtActionNode initialized", xml_tag_name.c_str()); + } - /** Callback invoked when the feedback is received. - * It generally returns RUNNING, but the user can also use this callback to cancel the - * current action and return SUCCESS or FAILURE. - */ - virtual BT::NodeStatus onFeeback(const std::shared_ptr feedback) + BtActionNode() = delete; + + virtual ~BtActionNode() { - return NodeStatus::RUNNING; } - /** Callback invoked when something goes wrong. - * It must return either SUCCESS or FAILURE. + /** + * @brief Create instance of an action client + * @param action_name Action name to create client for */ - virtual BT::NodeStatus onFailure(ActionNodeErrorCode error) + void createActionClient(const std::string & action_name) { - return NodeStatus::FAILURE; - } + // Now that we have the ROS node to use, create the action client for this BT action + action_client_ = rclcpp_action::create_client(node_, action_name, callback_group_); - /// Method used to send a request to the Action server to cancel the current goal - void cancelGoal(); + // Make sure the server is actually there before continuing + RCLCPP_DEBUG(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str()); + action_client_->wait_for_action_server(); + } -protected: + /** + * @brief Any subclass of BtActionNode that accepts parameters must provide a + * providedPorts method and call providedBasicPorts in it. + * @param addition Additional ports to add to BT port list + * @return PortsList Containing basic ports along with node-specific ports + */ + static PortsList providedBasicPorts(PortsList addition) + { + PortsList basic = { + InputPort("server_name", "Action server name"), + InputPort("server_timeout") + }; + basic.insert(addition.begin(), addition.end()); - std::shared_ptr node_; - const std::string action_name_; - const std::chrono::milliseconds server_timeout_; + return basic; + } -private: + /** + * @brief Creates list of BT ports + * @return PortsList Containing basic ports along with node-specific ports + */ + static PortsList providedPorts() + { + return providedBasicPorts({}); + } - typename std::shared_ptr action_client_; - rclcpp::CallbackGroup::SharedPtr callback_group_; + // Derived classes can override any of the following methods to hook into the + // processing for the action: on_tick, on_wait_for_result, and on_success - std::shared_future future_goal_handle_; - typename GoalHandle::SharedPtr goal_handle_; + /** + * @brief Function to perform some user-defined operation on tick + * Could do dynamic checks, such as getting updates to values on the blackboard + */ + virtual void on_tick() + { + } - rclcpp::Time time_goal_sent_; - NodeStatus on_feedback_state_change_; - bool goal_received_; - WrappedResult result_; -}; + /** + * @brief Function to perform some user-defined operation after a timeout + * waiting for a result that hasn't been received yet. Also provides access to + * the latest feedback message from the action server. Feedback will be nullptr + * in subsequent calls to this function if no new feedback is received while waiting for a result. + * @param feedback shared_ptr to latest feedback message, nullptr if no new feedback was received + */ + virtual void on_wait_for_result(std::shared_ptr/*feedback*/) + { + } -/// Method to register the service into a factory. -/// It gives you the opportunity to set the ros::NodeHandle. -template static - void RegisterRosAction(BT::BehaviorTreeFactory& factory, - const std::string& registration_ID, - const ActionNodeParams& params, - std::shared_ptr external_client = {} ) -{ - NodeBuilder builder = [=](const std::string& name, const NodeConfiguration& config) + /** + * @brief Function to perform some user-defined operation upon successful + * completion of the action. Could put a value on the blackboard. + * @return NodeStatus Returns SUCCESS by default, user may override return another value + */ + virtual NodeStatus on_success() { - return std::make_unique(name, config, params, external_client); - }; - - TreeNodeManifest manifest; - manifest.type = getType(); - manifest.ports = DerivedT::providedPorts(); - manifest.registration_ID = registration_ID; - const auto& basic_ports = DerivedT::providedPorts(); - manifest.ports.insert( basic_ports.begin(), basic_ports.end() ); - factory.registerBuilder( manifest, builder ); -} - -//---------------------------------------------------------------- -//---------------------- DEFINITIONS ----------------------------- -//---------------------------------------------------------------- - -template inline - RosActionNode::RosActionNode(const std::string & instance_name, - const BT::NodeConfiguration& conf, - const ActionNodeParams& params, - typename std::shared_ptr external_action_client): - BT::ActionNodeBase(instance_name, conf), - node_(params.nh), - action_name_(params.action_name), - server_timeout_(params.server_timeout) -{ - if( external_action_client ) + return NodeStatus::SUCCESS; + } + + /** + * @brief Function to perform some user-defined operation whe the action is aborted. + * @return NodeStatus Returns FAILURE by default, user may override return another value + */ + virtual NodeStatus on_aborted() { - action_client_ = external_action_client; + return NodeStatus::FAILURE; } - else + + /** + * @brief Function to perform some user-defined operation when the action is cancelled. + * @return NodeStatus Returns SUCCESS by default, user may override return another value + */ + virtual NodeStatus on_cancelled() { - callback_group_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - action_client_ = rclcpp_action::create_client(node_, action_name_, callback_group_); - - bool found = action_client_->wait_for_action_server(server_timeout_); - if(!found) - { - RCLCPP_ERROR(node_->get_logger(), - "Action server [%s] is not reachable. This will be checked only once", action_name_.c_str()); - } + return NodeStatus::SUCCESS; } -} -template inline - NodeStatus RosActionNode::tick() -{ - auto CheckStatus = [](NodeStatus status) + /** + * @brief The main override required by a BT action + * @return NodeStatus Status of tick execution + */ + NodeStatus tick() override { - if( status != NodeStatus::SUCCESS && status != NodeStatus::FAILURE ) - { - throw std::logic_error("RosActionNode: the callback must return either SUCCESS of FAILURE"); + // first step to be done only at the beginning of the Action + if (status() == NodeStatus::IDLE) { + // setting the status to RUNNING to notify the BT Loggers (if any) + setStatus(NodeStatus::RUNNING); + + // user defined callback + on_tick(); + + send_new_goal(); } - return status; - }; - // first step to be done only at the beginning of the Action - if (status() == BT::NodeStatus::IDLE) - { - setStatus(NodeStatus::RUNNING); + try { + // if new goal was sent and action server has not yet responded + // check the future goal handle + if (future_goal_handle_) { + auto elapsed = (node_->now() - time_goal_sent_).to_chrono(); + if (!is_future_goal_handle_complete(elapsed)) { + // return RUNNING if there is still some time before timeout happens + if (elapsed < server_timeout_) { + return NodeStatus::RUNNING; + } + // if server has taken more time than the specified timeout value return FAILURE + RCLCPP_WARN( + node_->get_logger(), + "Timed out while waiting for action server to acknowledge goal request for %s", + action_name_.c_str()); + future_goal_handle_.reset(); + return NodeStatus::FAILURE; + } + } - goal_received_ = false; - future_goal_handle_ = {}; - on_feedback_state_change_ = NodeStatus::RUNNING; - result_ = {}; + // The following code corresponds to the "RUNNING" loop + if (rclcpp::ok() && !goal_result_available_) { + // user defined callback. May modify the value of "goal_updated_" + on_wait_for_result(feedback_); - Goal goal; + // reset feedback to avoid stale information + feedback_.reset(); - if( !setGoal(goal) ) - { - return CheckStatus( onFailure(INVALID_GOAL) ); - } + auto goal_status = goal_handle_->get_status(); + if (goal_updated_ && (goal_status == action_msgs::msg::GoalStatus::STATUS_EXECUTING || + goal_status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED)) + { + goal_updated_ = false; + send_new_goal(); + auto elapsed = (node_->now() - time_goal_sent_).to_chrono(); + if (!is_future_goal_handle_complete(elapsed)) { + if (elapsed < server_timeout_) { + return NodeStatus::RUNNING; + } + RCLCPP_WARN( + node_->get_logger(), + "Timed out while waiting for action server to acknowledge goal request for %s", + action_name_.c_str()); + future_goal_handle_.reset(); + return NodeStatus::FAILURE; + } + } - typename ActionClient::SendGoalOptions goal_options; + callback_group_executor_.spin_some(); - //-------------------- - goal_options.feedback_callback = - [this](typename GoalHandle::SharedPtr, - const std::shared_ptr feedback) - { - on_feedback_state_change_ = onFeeback(feedback); - if( on_feedback_state_change_ == NodeStatus::IDLE) - { - throw std::logic_error("onFeeback must not retunr IDLE"); + // check if, after invoking spin_some(), we finally received the result + if (!goal_result_available_) { + // Yield this Action, returning RUNNING + return NodeStatus::RUNNING; + } } - emitStateChanged(); - }; - //-------------------- - goal_options.result_callback = - [this](const WrappedResult& result) - { - RCLCPP_INFO( node_->get_logger(), "result_callback" ); - result_ = result; - emitStateChanged(); - }; - //-------------------- - goal_options.goal_response_callback = - [this](std::shared_future const future_handle) - { - auto goal_handle_ = future_handle.get(); - if (!goal_handle_) + } catch (const std::runtime_error & e) { + if (e.what() == std::string("send_goal failed") || + e.what() == std::string("Goal was rejected by the action server")) { - RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server"); + // Action related failure that should not fail the tree, but the node + return NodeStatus::FAILURE; } else { - RCLCPP_INFO(node_->get_logger(), "Goal accepted by server, waiting for result"); + // Internal exception to propogate to the tree + throw e; } - }; - //-------------------- + } - future_goal_handle_ = action_client_->async_send_goal( goal, goal_options ); - time_goal_sent_ = node_->now(); + NodeStatus status; + switch (result_.code) { + case rclcpp_action::ResultCode::SUCCEEDED: + status = on_success(); + break; + + case rclcpp_action::ResultCode::ABORTED: + status = on_aborted(); + break; + + case rclcpp_action::ResultCode::CANCELED: + status = on_cancelled(); + break; - return NodeStatus::RUNNING; + default: + throw std::logic_error("BtActionNode::Tick: invalid status value"); + } + + goal_handle_.reset(); + return status; } - if (status() == NodeStatus::RUNNING) + /** + * @brief The other (optional) override required by a BT action. In this case, we + * make sure to cancel the ROS2 action if it is still running. + */ + void halt() override { - rclcpp::spin_some(node_); + if (should_cancel_goal()) { + auto future_cancel = action_client_->async_cancel_goal(goal_handle_); + if (callback_group_executor_.spin_until_future_complete(future_cancel, server_timeout_) != + rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR( + node_->get_logger(), + "Failed to cancel action server for %s", action_name_.c_str()); + } + } - // FIRST case: check if the goal request has a timeout - if( !goal_received_ ) - { - auto nodelay = std::chrono::milliseconds(0); - auto timeout = rclcpp::Duration::from_seconds( double(server_timeout_.count()) / 1000); + setStatus(NodeStatus::IDLE); + } - if (rclcpp::spin_until_future_complete(node_, future_goal_handle_, nodelay) != - rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_WARN( node_->get_logger(), "waiting goal confirmation" ); - if( (node_->now() - time_goal_sent_) > timeout ) - { - RCLCPP_WARN( node_->get_logger(), "TIMEOUT" ); - return CheckStatus( onFailure(SEND_GOAL_TIMEOUT) ); - } - else{ - return NodeStatus::RUNNING; +protected: + /** + * @brief Function to check if current goal should be cancelled + * @return bool True if current goal should be cancelled, false otherwise + */ + bool should_cancel_goal() + { + // Shut the node down if it is currently running + if (status() != NodeStatus::RUNNING) { + return false; + } + + // No need to cancel the goal if goal handle is invalid + if (!goal_handle_) { + return false; + } + + callback_group_executor_.spin_some(); + auto status = goal_handle_->get_status(); + + // Check if the goal is still executing + return status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED || + status == action_msgs::msg::GoalStatus::STATUS_EXECUTING; + } + + /** + * @brief Function to send new goal to action server + */ + void send_new_goal() + { + goal_result_available_ = false; + auto send_goal_options = typename rclcpp_action::Client::SendGoalOptions(); + send_goal_options.result_callback = + [this](const typename rclcpp_action::ClientGoalHandle::WrappedResult & result) { + if (future_goal_handle_) { + RCLCPP_DEBUG( + node_->get_logger(), + "Goal result for %s available, but it hasn't received the goal response yet. " + "It's probably a goal result for the last goal request", action_name_.c_str()); + return; } - } - else - { - goal_received_ = true; - goal_handle_ = future_goal_handle_.get(); - future_goal_handle_ = {}; - if (!goal_handle_) { - throw std::runtime_error("Goal was rejected by the action server"); + // TODO(#1652): a work around until rcl_action interface is updated + // if goal ids are not matched, the older goal call this callback so ignore the result + // if matched, it must be processed (including aborted) + if (this->goal_handle_->get_goal_id() == result.goal_id) { + goal_result_available_ = true; + result_ = result; } - } + }; + send_goal_options.feedback_callback = + [this](typename rclcpp_action::ClientGoalHandle::SharedPtr, + const std::shared_ptr feedback) { + feedback_ = feedback; + }; + + future_goal_handle_ = std::make_shared< + std::shared_future::SharedPtr>>( + action_client_->async_send_goal(goal_, send_goal_options)); + time_goal_sent_ = node_->now(); + } + + /** + * @brief Function to check if the action server acknowledged a new goal + * @param elapsed Duration since the last goal was sent and future goal handle has not completed. + * After waiting for the future to complete, this value is incremented with the timeout value. + * @return boolean True if future_goal_handle_ returns SUCCESS, False otherwise + */ + bool is_future_goal_handle_complete(std::chrono::milliseconds & elapsed) + { + auto remaining = server_timeout_ - elapsed; + + // server has already timed out, no need to sleep + if (remaining <= std::chrono::milliseconds(0)) { + future_goal_handle_.reset(); + return false; } - // SECOND case: onFeeback requested a stop - if( on_feedback_state_change_ != NodeStatus::RUNNING ) - { - cancelGoal(); - return on_feedback_state_change_; + auto timeout = remaining > bt_loop_duration_ ? bt_loop_duration_ : remaining; + auto result = + callback_group_executor_.spin_until_future_complete(*future_goal_handle_, timeout); + elapsed += timeout; + + if (result == rclcpp::FutureReturnCode::INTERRUPTED) { + future_goal_handle_.reset(); + throw std::runtime_error("send_goal failed"); } - // THIRD case: result received, requested a stop - if( result_.code != rclcpp_action::ResultCode::UNKNOWN) - { - if( result_.code == rclcpp_action::ResultCode::ABORTED ) - { - return CheckStatus( onFailure( ACTION_ABORTED ) ); - } - else if( result_.code == rclcpp_action::ResultCode::CANCELED ) - { - return CheckStatus( onFailure( ACTION_CANCELLED ) ); - } - else{ - return CheckStatus( onResultReceived( result_ ) ); + + if (result == rclcpp::FutureReturnCode::SUCCESS) { + goal_handle_ = future_goal_handle_->get(); + future_goal_handle_.reset(); + if (!goal_handle_) { + throw std::runtime_error("Goal was rejected by the action server"); } + return true; } + + return false; } - return NodeStatus::RUNNING; -} -template inline - void RosActionNode::halt() -{ - if( status() == NodeStatus::RUNNING ) + /** + * @brief Function to increment recovery count on blackboard if this node wraps a recovery + */ + void increment_recovery_count() { - cancelGoal(); + int recovery_count = 0; + config().blackboard->template get("number_recoveries", recovery_count); // NOLINT + recovery_count += 1; + config().blackboard->template set("number_recoveries", recovery_count); // NOLINT } -} -template inline - void RosActionNode::cancelGoal() -{ - auto future_cancel = action_client_->async_cancel_goal(goal_handle_); + std::string action_name_; + typename std::shared_ptr> action_client_; - if (rclcpp::spin_until_future_complete(node_, future_cancel, server_timeout_) != - rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_ERROR( node_->get_logger(), - "Failed to cancel action server for %s", action_name_.c_str()); - } -} + // All ROS2 actions have a goal and a result + typename ActionT::Goal goal_; + bool goal_updated_{false}; + bool goal_result_available_{false}; + typename rclcpp_action::ClientGoalHandle::SharedPtr goal_handle_; + typename rclcpp_action::ClientGoalHandle::WrappedResult result_; + // To handle feedback from action server + std::shared_ptr feedback_; + + // The node that will be used for any ROS operations + rclcpp::Node::SharedPtr node_; + rclcpp::CallbackGroup::SharedPtr callback_group_; + rclcpp::executors::SingleThreadedExecutor callback_group_executor_; + // The timeout value while waiting for response from a server when a + // new action goal is sent or canceled + std::chrono::milliseconds server_timeout_; + // The timeout value for BT loop execution + std::chrono::milliseconds bt_loop_duration_; + + // To track the action server acknowledgement when a new goal is sent + std::shared_ptr::SharedPtr>> + future_goal_handle_; + rclcpp::Time time_goal_sent_; +}; } // namespace BT diff --git a/include/behaviortree_ros2/bt_conversions.hpp b/include/behaviortree_ros2/bt_conversions.hpp new file mode 100644 index 0000000..6d909ee --- /dev/null +++ b/include/behaviortree_ros2/bt_conversions.hpp @@ -0,0 +1,192 @@ +// Copyright (c) 2018 Intel Corporation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BEHAVIOR_TREE_ROS2__BT_CONVERSIONS_HPP_ +#define BEHAVIOR_TREE_ROS2__BT_CONVERSIONS_HPP_ + +#include +#include +#include +#include +#include + +#include "behaviortree_cpp/behavior_tree.h" + +namespace BT +{ +/// @brief Custom type +struct Vector4D +{ + double x{0.0}, y{0.0}, z{0.0}, w{0.0}; +}; + +// The follow templates are required when using these types as parameters +// in our BT XML files. They parse the strings in the XML into their corresponding +// data type. + +/** + * @brief Parse XML string to geometry_msgs::msg::Point + * @param key XML string + * @return geometry_msgs::msg::Point + */ +template<> +inline geometry_msgs::msg::Point convertFromString(const StringView key) +{ + // three real numbers separated by semicolons + auto parts = BT::splitString(key, ';'); + if (parts.size() != 3) { + throw std::runtime_error("invalid number of fields for point attribute)"); + } else { + geometry_msgs::msg::Point position; + position.x = BT::convertFromString(parts[0]); + position.y = BT::convertFromString(parts[1]); + position.z = BT::convertFromString(parts[2]); + return position; + } +} + +/** + * @brief Parse XML string to geometry_msgs::msg::Quaternion + * @param key XML string + * @return geometry_msgs::msg::Quaternion + */ +template<> +inline geometry_msgs::msg::Quaternion convertFromString(const StringView key) +{ + // four real numbers separated by semicolons + auto parts = BT::splitString(key, ';'); + if (parts.size() != 4) { + throw std::runtime_error("invalid number of fields for orientation attribute)"); + } else { + geometry_msgs::msg::Quaternion orientation; + orientation.x = BT::convertFromString(parts[0]); + orientation.y = BT::convertFromString(parts[1]); + orientation.z = BT::convertFromString(parts[2]); + orientation.w = BT::convertFromString(parts[3]); + return orientation; + } +} + +/** + * @brief Parse XML string to geometry_msgs::msg::PoseStamped + * @param key XML string + * @return geometry_msgs::msg::PoseStamped + */ +template<> +inline geometry_msgs::msg::PoseStamped convertFromString(const StringView key) +{ + // 7 real numbers separated by semicolons + auto parts = BT::splitString(key, ';'); + if (parts.size() != 9) { + throw std::runtime_error("invalid number of fields for PoseStamped attribute)"); + } else { + geometry_msgs::msg::PoseStamped pose_stamped; + pose_stamped.header.stamp = rclcpp::Time(BT::convertFromString(parts[0])); + pose_stamped.header.frame_id = BT::convertFromString(parts[1]); + pose_stamped.pose.position.x = BT::convertFromString(parts[2]); + pose_stamped.pose.position.y = BT::convertFromString(parts[3]); + pose_stamped.pose.position.z = BT::convertFromString(parts[4]); + pose_stamped.pose.orientation.x = BT::convertFromString(parts[5]); + pose_stamped.pose.orientation.y = BT::convertFromString(parts[6]); + pose_stamped.pose.orientation.z = BT::convertFromString(parts[7]); + pose_stamped.pose.orientation.w = BT::convertFromString(parts[8]); + return pose_stamped; + } +} + +/** + * @brief Parse XML string to std::chrono::milliseconds + * @param key XML string + * @return std::chrono::milliseconds + */ +template<> +inline std::chrono::milliseconds convertFromString(const StringView key) +{ + return std::chrono::milliseconds(std::stoul(key.data())); +} + +/** + * @brief Parse XML string to custom Vector4D + * by comma-separated-values. + * FORMAT: X,Y,Z,W;X,Y,Z,W; ... + * @param key XML string + * @return Vector4D + */ +template <> inline +Vector4D convertFromString(StringView key) +{ + // three real numbers separated by colons + auto parts = BT::splitString(key, ','); + if (parts.size() != 4) + { + throw BT::RuntimeError("invalid input)"); + } + else + { + Vector4D output; + output.x = convertFromString(parts[0]); + output.y = convertFromString(parts[1]); + output.z = convertFromString(parts[2]); + output.w = convertFromString(parts[3]); + return output; + } +} + +/** + * @brief Parse XML string to custom std::vector of Vector4D + * by comma-separated-values. + * FORMAT: X,Y,Z,W;X,Y,Z,W; ... + * @param key XML string + * @return std::vector + */ +template <> inline +std::vector convertFromString(StringView key) +{ + std::vector output_vector; + auto parts_vector = BT::splitString(key, ';'); + + for (auto& part_vector : parts_vector) + { + Vector4D output = convertFromString(part_vector); + output_vector.push_back(output); + } + return output_vector; + +} + +/** + * @brief Parse XML string to custom std::vector of string + * by colon-separated-values. + * FORMAT: str;str; ... + * @param key XML string + * @return std::vector + */ +template <> inline +std::vector convertFromString(StringView key) +{ + std::vector output_vector; + auto parts_vector = BT::splitString(key, ';'); + + for (auto& part_vector : parts_vector) + { + std::string output = convertFromString(part_vector); + output_vector.push_back(output); + } + return output_vector; + +} + +} // namespace BT + +#endif // BEHAVIOR_TREE_ROS2__BT_CONVERSIONS_HPP_ diff --git a/include/behaviortree_ros2/bt_service_node.hpp b/include/behaviortree_ros2/bt_service_node.hpp new file mode 100644 index 0000000..0f516e0 --- /dev/null +++ b/include/behaviortree_ros2/bt_service_node.hpp @@ -0,0 +1,241 @@ +// Copyright (c) 2019 Samsung Research America +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BEHAVIOR_TREE_ROS2__BT_ACTION_NODE_HPP_ +#define BEHAVIOR_TREE_ROS2__BT_ACTION_NODE_HPP_ + +#include +#include +#include + +#include "behaviortree_cpp/action_node.h" +#include "behaviortree_ros2/bt_conversions.hpp" + +namespace BT +{ + +/** + * @brief Abstract class representing a service based BT node + * @tparam ServiceT Type of service + */ +template +class BtServiceNode : public ActionNodeBase +{ +public: + /** + * @brief A nav2_behavior_tree::BtServiceNode constructor + * @param service_node_name Service name this node creates a client for + * @param conf BT node configuration + */ + BtServiceNode( + const std::string & service_node_name, + const NodeConfig& conf) + : ActionNodeBase(service_node_name, conf), service_node_name_(service_node_name) + { + node_ = config().blackboard->template get("node"); + callback_group_ = node_->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, + false); + callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); + + // Get the required items from the blackboard + bt_loop_duration_ = + config().blackboard->template get("bt_loop_duration"); + server_timeout_ = + config().blackboard->template get("server_timeout"); + getInput("server_timeout", server_timeout_); + + // Now that we have node_ to use, create the service client for this BT service + getInput("service_name", service_name_); + std::string node_namespace; + node_namespace = node_->get_namespace(); + // Append namespace to the action name + if(node_namespace != "/") { + service_name_ = node_namespace + "/" + service_name_; + } + service_client_ = node_->create_client( + service_name_, + rmw_qos_profile_services_default, + callback_group_); + + // Make a request for the service without parameter + request_ = std::make_shared(); + + // Make sure the server is actually there before continuing + RCLCPP_DEBUG( + node_->get_logger(), "Waiting for \"%s\" service", + service_name_.c_str()); + service_client_->wait_for_service(); + + RCLCPP_DEBUG( + node_->get_logger(), "\"%s\" BtServiceNode initialized", + service_node_name_.c_str()); + } + + BtServiceNode() = delete; + + virtual ~BtServiceNode() + { + } + + /** + * @brief Any subclass of BtServiceNode that accepts parameters must provide a + * providedPorts method and call providedBasicPorts in it. + * @param addition Additional ports to add to BT port list + * @return PortsList Containing basic ports along with node-specific ports + */ + static PortsList providedBasicPorts(PortsList addition) + { + PortsList basic = { + InputPort("service_name", "please_set_service_name_in_BT_Node"), + InputPort("server_timeout") + }; + basic.insert(addition.begin(), addition.end()); + + return basic; + } + + /** + * @brief Creates list of BT ports + * @return PortsList Containing basic ports along with node-specific ports + */ + static PortsList providedPorts() + { + return providedBasicPorts({}); + } + + /** + * @brief The main override required by a BT service + * @return NodeStatus Status of tick execution + */ + NodeStatus tick() override + { + if (!request_sent_) { + on_tick(); + future_result_ = service_client_->async_send_request(request_).share(); + sent_time_ = node_->now(); + request_sent_ = true; + } + return check_future(); + } + + /** + * @brief The other (optional) override required by a BT service. + */ + void halt() override + { + request_sent_ = false; + setStatus(NodeStatus::IDLE); + } + + /** + * @brief Function to perform some user-defined operation on tick + * Fill in service request with information if necessary + */ + virtual void on_tick() + { + } + + /** + * @brief Function to perform some user-defined operation upon successful + * completion of the service. Could put a value on the blackboard. + * @param response can be used to get the result of the service call in the BT Node. + * @return NodeStatus Returns SUCCESS by default, user may override to return another value + */ + virtual NodeStatus on_completion(std::shared_ptr/*response*/) + { + return NodeStatus::SUCCESS; + } + + /** + * @brief Check the future and decide the status of BT + * @return NodeStatus SUCCESS if future complete before timeout, FAILURE otherwise + */ + virtual NodeStatus check_future() + { + auto elapsed = (node_->now() - sent_time_).to_chrono(); + auto remaining = server_timeout_ - elapsed; + + if (remaining > std::chrono::milliseconds(0)) { + auto timeout = remaining > bt_loop_duration_ ? bt_loop_duration_ : remaining; + + rclcpp::FutureReturnCode rc; + rc = callback_group_executor_.spin_until_future_complete(future_result_, server_timeout_); + if (rc == rclcpp::FutureReturnCode::SUCCESS) { + request_sent_ = false; + NodeStatus status = on_completion(future_result_.get()); + return status; + } + + if (rc == rclcpp::FutureReturnCode::TIMEOUT) { + on_wait_for_result(); + elapsed = (node_->now() - sent_time_).to_chrono(); + if (elapsed < server_timeout_) { + return NodeStatus::RUNNING; + } + } + } + + RCLCPP_WARN( + node_->get_logger(), + "Node timed out while executing service call to %s.", service_name_.c_str()); + request_sent_ = false; + return NodeStatus::FAILURE; + } + + /** + * @brief Function to perform some user-defined operation after a timeout waiting + * for a result that hasn't been received yet + */ + virtual void on_wait_for_result() + { + } + +protected: + /** + * @brief Function to increment recovery count on blackboard if this node wraps a recovery + */ + void increment_recovery_count() + { + int recovery_count = 0; + config().blackboard->template get("number_recoveries", recovery_count); // NOLINT + recovery_count += 1; + config().blackboard->template set("number_recoveries", recovery_count); // NOLINT + } + + std::string service_name_, service_node_name_; + typename std::shared_ptr> service_client_; + std::shared_ptr request_; + + // The node that will be used for any ROS operations + rclcpp::Node::SharedPtr node_; + rclcpp::CallbackGroup::SharedPtr callback_group_; + rclcpp::executors::SingleThreadedExecutor callback_group_executor_; + + // The timeout value while to use in the tick loop while waiting for + // a result from the server + std::chrono::milliseconds server_timeout_; + + // The timeout value for BT loop execution + std::chrono::milliseconds bt_loop_duration_; + + // To track the server response when a new request is sent + std::shared_future future_result_; + bool request_sent_{false}; + rclcpp::Time sent_time_; +}; + +} // namespace BT + +#endif // BEHAVIOR_TREE_ROS2__BT_ACTION_NODE_HPP_ diff --git a/package.xml b/package.xml index 36d23ae..84c8768 100644 --- a/package.xml +++ b/package.xml @@ -11,13 +11,34 @@ ament_cmake - rclcpp - rclcpp_action - behaviortree_cpp_v3 + libboost-dev + rclcpp + rclcpp_action + rclcpp_lifecycle + behaviortree_cpp + builtin_interfaces + geometry_msgs + sensor_msgs + tf2 + tf2_ros + tf2_geometry_msgs + std_msgs + std_srvs + lifecycle_msgs - rosidl_default_generators - action_msgs - rosidl_interface_packages + libboost-dev + rclcpp + rclcpp_action + rclcpp_lifecycle + std_msgs + behaviortree_cpp + builtin_interfaces + geometry_msgs + sensor_msgs + tf2 + tf2_ros + tf2_geometry_msgs + lifecycle_msgs ament_cmake diff --git a/src/behavior_tree_engine.cpp b/src/behavior_tree_engine.cpp new file mode 100644 index 0000000..310f0f8 --- /dev/null +++ b/src/behavior_tree_engine.cpp @@ -0,0 +1,106 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Florian Gramss +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + + +#include +#include +#include +#include + +#include "behaviortree_cpp/utils/shared_library.h" +#include "behaviortree_ros2/behavior_tree_engine.hpp" + +namespace BT +{ + +BehaviorTreeEngine::BehaviorTreeEngine(const std::vector & plugin_libraries) +{ + SharedLibrary loader; + for (const auto & p : plugin_libraries) { + factory_.registerFromPlugin(loader.getOSName(p)); + } +} + +BtStatus +BehaviorTreeEngine::run( + Tree * tree, + std::function onLoop, + std::function cancelRequested, + std::chrono::milliseconds loopTimeout) +{ + rclcpp::WallRate loopRate(loopTimeout); + NodeStatus result = NodeStatus::RUNNING; + + // Loop until something happens with ROS or the node completes + try { + while (rclcpp::ok() && result == NodeStatus::RUNNING) { + if (cancelRequested()) { + tree->rootNode()->haltNode(); + return BtStatus::CANCELED; + } + + result = tree->tickOnce(); + + onLoop(); + + loopRate.sleep(); + } + } catch (const std::exception & ex) { + RCLCPP_ERROR( + rclcpp::get_logger("BehaviorTreeEngine"), + "Behavior tree threw exception: %s. Exiting with failure.", ex.what()); + return BtStatus::FAILED; + } + + return (result == NodeStatus::SUCCESS) ? BtStatus::SUCCEEDED : BtStatus::FAILED; +} + +Tree +BehaviorTreeEngine::createTreeFromText( + const std::string & xml_string, + Blackboard::Ptr blackboard) +{ + return factory_.createTreeFromText(xml_string, blackboard); +} + +Tree +BehaviorTreeEngine::createTreeFromFile( + const std::string & file_path, + Blackboard::Ptr blackboard) +{ + return factory_.createTreeFromFile(file_path, blackboard); +} + +// In order to re-run a Behavior Tree, we must be able to reset all nodes to the initial state +void +BehaviorTreeEngine::haltAllActions(TreeNode * root_node) +{ + if (!root_node) { + return; + } + + // this halt signal should propagate through the entire tree. + root_node->haltNode(); + + // but, just in case... + auto visitor = [](TreeNode * node) { + if (node->status() == NodeStatus::RUNNING) { + node->haltNode(); + } + }; + applyRecursiveVisitor(root_node, visitor); +} + +} // namespace BT diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt new file mode 100644 index 0000000..1486afa --- /dev/null +++ b/test/CMakeLists.txt @@ -0,0 +1,7 @@ +ament_add_gtest(test_bt_conversions test_bt_conversions.cpp) +ament_target_dependencies(test_bt_conversions ${dependencies}) + +add_subdirectory(plugins/condition) +add_subdirectory(plugins/decorator) +add_subdirectory(plugins/control) +add_subdirectory(plugins/action) diff --git a/test/sleep_server.cpp b/test/sleep_server.cpp deleted file mode 100644 index dd1e6db..0000000 --- a/test/sleep_server.cpp +++ /dev/null @@ -1,103 +0,0 @@ -#include -#include -#include - -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_action/rclcpp_action.hpp" -#include "behaviortree_ros2/action/sleep.hpp" - -#include "behaviortree_ros2/bt_action_node.hpp" - -class SleepActionServer : public rclcpp::Node -{ -public: - using Sleep = behaviortree_ros2::action::Sleep; - using GoalHandleSleep = rclcpp_action::ServerGoalHandle; - - explicit SleepActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) - : Node("sleep_action_server", options) - { - using namespace std::placeholders; - - this->action_server_ = rclcpp_action::create_server( - this, - "sleep_service", - std::bind(&SleepActionServer::handle_goal, this, _1, _2), - std::bind(&SleepActionServer::handle_cancel, this, _1), - std::bind(&SleepActionServer::handle_accepted, this, _1)); - } - -private: - rclcpp_action::Server::SharedPtr action_server_; - - rclcpp_action::GoalResponse handle_goal( - const rclcpp_action::GoalUUID &, - std::shared_ptr goal) - { - RCLCPP_INFO(this->get_logger(), "Received goal request with sleep time %d", goal->msec_timeout); - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; - } - - rclcpp_action::CancelResponse handle_cancel( - const std::shared_ptr goal_handle) - { - RCLCPP_INFO(this->get_logger(), "Received request to cancel goal"); - (void)goal_handle; - return rclcpp_action::CancelResponse::ACCEPT; - } - - void handle_accepted(const std::shared_ptr goal_handle) - { - using namespace std::placeholders; - // this needs to return quickly to avoid blocking the executor, so spin up a new thread - std::thread{std::bind(&SleepActionServer::execute, this, _1), goal_handle}.detach(); - } - - void execute(const std::shared_ptr goal_handle) - { - RCLCPP_INFO(this->get_logger(), "Executing goal"); - rclcpp::Rate loop_rate(5); - const auto goal = goal_handle->get_goal(); - auto feedback = std::make_shared(); - auto result = std::make_shared(); - - rclcpp::Time deadline = get_clock()->now() + rclcpp::Duration::from_seconds( double(goal->msec_timeout) / 1000 ); - int cycle = 0; - - while( get_clock()->now() < deadline ) - { - if (goal_handle->is_canceling()) - { - result->done = false; - goal_handle->canceled(result); - RCLCPP_INFO(this->get_logger(), "Goal canceled"); - return; - } - - feedback->cycle = cycle++; - goal_handle->publish_feedback(feedback); - RCLCPP_INFO(this->get_logger(), "Publish feedback"); - - loop_rate.sleep(); - } - - // Check if goal is done - if (rclcpp::ok()) - { - result->done = true; - goal_handle->succeed(result); - RCLCPP_INFO(this->get_logger(), "Goal succeeded"); - } - } -}; // class SleepActionServer - - -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - auto node = std::make_shared(); - - rclcpp::spin(node); - - return 0; -} diff --git a/test/test_action_server.hpp b/test/test_action_server.hpp new file mode 100644 index 0000000..d532a7d --- /dev/null +++ b/test/test_action_server.hpp @@ -0,0 +1,100 @@ +// Copyright (c) 2020 Sarthak Mittal +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_ACTION_SERVER_HPP_ +#define TEST_ACTION_SERVER_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" + +template +class TestActionServer : public rclcpp::Node +{ +public: + explicit TestActionServer( + std::string action_name, + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) + : Node("test_action_server", options) + { + using namespace std::placeholders; // NOLINT + + this->action_server_ = rclcpp_action::create_server( + this->get_node_base_interface(), + this->get_node_clock_interface(), + this->get_node_logging_interface(), + this->get_node_waitables_interface(), + action_name, + std::bind(&TestActionServer::handle_goal, this, _1, _2), + std::bind(&TestActionServer::handle_cancel, this, _1), + std::bind(&TestActionServer::handle_accepted, this, _1)); + } + + std::shared_ptr getCurrentGoal() const + { + return current_goal_; + } + + void setReturnSuccess(bool return_success) + { + return_success_ = return_success; + } + + bool getReturnSuccess(void) + { + return return_success_; + } + + bool isGoalCancelled() + { + return goal_cancelled_; + } + +protected: + virtual rclcpp_action::GoalResponse handle_goal( + const rclcpp_action::GoalUUID &, + std::shared_ptr goal) + { + current_goal_ = goal; + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } + + virtual rclcpp_action::CancelResponse handle_cancel( + const typename std::shared_ptr>) + { + goal_cancelled_ = true; + return rclcpp_action::CancelResponse::ACCEPT; + } + + virtual void execute( + const typename std::shared_ptr> goal_handle) = 0; + + void handle_accepted( + const std::shared_ptr> goal_handle) + { + using namespace std::placeholders; // NOLINT + // this needs to return quickly to avoid blocking the executor, so spin up a new thread + std::thread{std::bind(&TestActionServer::execute, this, _1), goal_handle}.detach(); + } + +private: + typename rclcpp_action::Server::SharedPtr action_server_; + std::shared_ptr current_goal_; + bool return_success_ = true; + bool goal_cancelled_ = false; +}; + +#endif // TEST_ACTION_SERVER_HPP_ diff --git a/test/test_behavior_tree_fixture.hpp b/test/test_behavior_tree_fixture.hpp new file mode 100644 index 0000000..2a377f9 --- /dev/null +++ b/test/test_behavior_tree_fixture.hpp @@ -0,0 +1,93 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Sarthak Mittal +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_BEHAVIOR_TREE_FIXTURE_HPP_ +#define TEST_BEHAVIOR_TREE_FIXTURE_HPP_ + +#include +#include +#include + +#include "behaviortree_cpp_v3/bt_factory.h" +#include "rclcpp/rclcpp.hpp" + +#include "test_transform_handler.hpp" +#include "test_dummy_tree_node.hpp" + +namespace nav2_behavior_tree +{ + +class BehaviorTreeTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("test_behavior_tree_fixture"); + transform_handler_ = std::make_shared(node_); + factory_ = std::make_shared(); + + config_ = new BT::NodeConfiguration(); + + // Create the blackboard that will be shared by all of the nodes in the tree + config_->blackboard = BT::Blackboard::create(); + // Put items on the blackboard + config_->blackboard->set( + "node", + node_); + config_->blackboard->set>( + "tf_buffer", + transform_handler_->getBuffer()); + config_->blackboard->set( + "server_timeout", + std::chrono::milliseconds(20)); + config_->blackboard->set( + "bt_loop_duration", + std::chrono::milliseconds(10)); + config_->blackboard->set("initial_pose_received", false); + + transform_handler_->activate(); + transform_handler_->waitForTransform(); + } + + static void TearDownTestCase() + { + transform_handler_->deactivate(); + delete config_; + config_ = nullptr; + transform_handler_.reset(); + node_.reset(); + factory_.reset(); + } + +protected: + static rclcpp::Node::SharedPtr node_; + static std::shared_ptr transform_handler_; + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; +}; + +} // namespace nav2_behavior_tree + +rclcpp::Node::SharedPtr nav2_behavior_tree::BehaviorTreeTestFixture::node_ = nullptr; + +std::shared_ptr +nav2_behavior_tree::BehaviorTreeTestFixture::transform_handler_ = nullptr; + +BT::NodeConfiguration * nav2_behavior_tree::BehaviorTreeTestFixture::config_ = nullptr; + +std::shared_ptr +nav2_behavior_tree::BehaviorTreeTestFixture::factory_ = nullptr; + +#endif // TEST_BEHAVIOR_TREE_FIXTURE_HPP_ diff --git a/test/test_bt_conversions.cpp b/test/test_bt_conversions.cpp new file mode 100644 index 0000000..ab67d5e --- /dev/null +++ b/test/test_bt_conversions.cpp @@ -0,0 +1,269 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Sarthak Mittal +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "geometry_msgs/msg/point.hpp" +#include "geometry_msgs/msg/quaternion.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" + +#include "behaviortree_cpp_v3/bt_factory.h" +#include "nav2_behavior_tree/bt_conversions.hpp" + +template +class TestNode : public BT::SyncActionNode +{ +public: + TestNode(const std::string & name, const BT::NodeConfiguration & config) + : SyncActionNode(name, config) + {} + + BT::NodeStatus tick() override + { + return BT::NodeStatus::SUCCESS; + } + + static BT::PortsList providedPorts() + { + return { + BT::InputPort("test") + }; + } +}; + + +TEST(PointPortTest, test_wrong_syntax) +{ + std::string xml_txt = + R"( + + + + + )"; + + BT::BehaviorTreeFactory factory; + factory.registerNodeType>("PointPort"); + auto tree = factory.createTreeFromText(xml_txt); + + geometry_msgs::msg::Point value; + tree.rootNode()->getInput("test", value); + EXPECT_EQ(value.x, 0.0); + EXPECT_EQ(value.y, 0.0); + EXPECT_EQ(value.z, 0.0); + + xml_txt = + R"( + + + + + )"; + + tree = factory.createTreeFromText(xml_txt); + tree.rootNode()->getInput("test", value); + EXPECT_EQ(value.x, 0.0); + EXPECT_EQ(value.y, 0.0); + EXPECT_EQ(value.z, 0.0); +} + +TEST(PointPortTest, test_correct_syntax) +{ + std::string xml_txt = + R"( + + + + + )"; + + BT::BehaviorTreeFactory factory; + factory.registerNodeType>("PointPort"); + auto tree = factory.createTreeFromText(xml_txt); + + geometry_msgs::msg::Point value; + tree.rootNode()->getInput("test", value); + EXPECT_EQ(value.x, 1.0); + EXPECT_EQ(value.y, 2.0); + EXPECT_EQ(value.z, 3.0); +} + +TEST(QuaternionPortTest, test_wrong_syntax) +{ + std::string xml_txt = + R"( + + + + + )"; + + BT::BehaviorTreeFactory factory; + factory.registerNodeType>("QuaternionPort"); + auto tree = factory.createTreeFromText(xml_txt); + + geometry_msgs::msg::Quaternion value; + tree.rootNode()->getInput("test", value); + EXPECT_EQ(value.x, 0.0); + EXPECT_EQ(value.y, 0.0); + EXPECT_EQ(value.z, 0.0); + EXPECT_EQ(value.w, 1.0); + + xml_txt = + R"( + + + + + )"; + + tree = factory.createTreeFromText(xml_txt); + tree.rootNode()->getInput("test", value); + EXPECT_EQ(value.x, 0.0); + EXPECT_EQ(value.y, 0.0); + EXPECT_EQ(value.z, 0.0); + EXPECT_EQ(value.w, 1.0); +} + +TEST(QuaternionPortTest, test_correct_syntax) +{ + std::string xml_txt = + R"( + + + + + )"; + + BT::BehaviorTreeFactory factory; + factory.registerNodeType>("QuaternionPort"); + auto tree = factory.createTreeFromText(xml_txt); + + geometry_msgs::msg::Quaternion value; + tree.rootNode()->getInput("test", value); + EXPECT_EQ(value.x, 0.7); + EXPECT_EQ(value.y, 0.0); + EXPECT_EQ(value.z, 0.0); + EXPECT_EQ(value.w, 0.7); +} + +TEST(PoseStampedPortTest, test_wrong_syntax) +{ + std::string xml_txt = + R"( + + + + + )"; + + BT::BehaviorTreeFactory factory; + factory.registerNodeType>("PoseStampedPort"); + auto tree = factory.createTreeFromText(xml_txt); + + geometry_msgs::msg::PoseStamped value; + tree.rootNode()->getInput("test", value); + EXPECT_EQ(rclcpp::Time(value.header.stamp).nanoseconds(), 0); + EXPECT_EQ(value.header.frame_id, ""); + EXPECT_EQ(value.pose.position.x, 0.0); + EXPECT_EQ(value.pose.position.y, 0.0); + EXPECT_EQ(value.pose.position.z, 0.0); + EXPECT_EQ(value.pose.orientation.x, 0.0); + EXPECT_EQ(value.pose.orientation.y, 0.0); + EXPECT_EQ(value.pose.orientation.z, 0.0); + EXPECT_EQ(value.pose.orientation.w, 1.0); + + xml_txt = + R"( + + + + + )"; + + tree = factory.createTreeFromText(xml_txt); + tree.rootNode()->getInput("test", value); + EXPECT_EQ(rclcpp::Time(value.header.stamp).nanoseconds(), 0); + EXPECT_EQ(value.header.frame_id, ""); + EXPECT_EQ(value.pose.position.x, 0.0); + EXPECT_EQ(value.pose.position.y, 0.0); + EXPECT_EQ(value.pose.position.z, 0.0); + EXPECT_EQ(value.pose.orientation.x, 0.0); + EXPECT_EQ(value.pose.orientation.y, 0.0); + EXPECT_EQ(value.pose.orientation.z, 0.0); + EXPECT_EQ(value.pose.orientation.w, 1.0); +} + +TEST(PoseStampedPortTest, test_correct_syntax) +{ + std::string xml_txt = + R"( + + + + + )"; + + BT::BehaviorTreeFactory factory; + factory.registerNodeType>("PoseStampedPort"); + auto tree = factory.createTreeFromText(xml_txt); + + tree = factory.createTreeFromText(xml_txt); + geometry_msgs::msg::PoseStamped value; + tree.rootNode()->getInput("test", value); + EXPECT_EQ(rclcpp::Time(value.header.stamp).nanoseconds(), 0); + EXPECT_EQ(value.header.frame_id, "map"); + EXPECT_EQ(value.pose.position.x, 1.0); + EXPECT_EQ(value.pose.position.y, 2.0); + EXPECT_EQ(value.pose.position.z, 3.0); + EXPECT_EQ(value.pose.orientation.x, 4.0); + EXPECT_EQ(value.pose.orientation.y, 5.0); + EXPECT_EQ(value.pose.orientation.z, 6.0); + EXPECT_EQ(value.pose.orientation.w, 7.0); +} + +TEST(MillisecondsPortTest, test_correct_syntax) +{ + std::string xml_txt = + R"( + + + + + )"; + + BT::BehaviorTreeFactory factory; + factory.registerNodeType>("MillisecondsPort"); + auto tree = factory.createTreeFromText(xml_txt); + + std::chrono::milliseconds value; + tree.rootNode()->getInput("test", value); + EXPECT_EQ(value.count(), 10000); + + xml_txt = + R"( + + + + + )"; + + tree = factory.createTreeFromText(xml_txt); + tree.rootNode()->getInput("test", value); + EXPECT_EQ(value.count(), 123); +} diff --git a/test/test_client.cpp b/test/test_client.cpp deleted file mode 100644 index 0b72aad..0000000 --- a/test/test_client.cpp +++ /dev/null @@ -1,116 +0,0 @@ -#include -#include -#include - -#include "behaviortree_ros2/action/sleep.hpp" - -using namespace BT; - -//------------------------------------------------------------- -// Simple Action to print a number -//------------------------------------------------------------- - -class PrintValue : public BT::SyncActionNode -{ -public: - PrintValue(const std::string& name, const BT::NodeConfiguration& config) - : BT::SyncActionNode(name, config) {} - - BT::NodeStatus tick() override { - std::string msg; - if( getInput("message", msg ) ){ - std::cout << "PrintValue: " << msg << std::endl; - return NodeStatus::SUCCESS; - } - else{ - std::cout << "PrintValue FAILED "<< std::endl; - return NodeStatus::FAILURE; - } - } - - static BT::PortsList providedPorts() { - return{ BT::InputPort("message") }; - } -}; - -class SleepAction: public RosActionNode -{ -public: - SleepAction(const std::string& name, - const BT::NodeConfiguration& conf, - const ActionNodeParams& params, - typename std::shared_ptr action_client) - : RosActionNode(name, conf, params, action_client) - {} - - static BT::PortsList providedPorts() - { - return {InputPort("msec")}; - } - - bool setGoal(Goal& goal) override - { - auto timeout = getInput("msec"); - goal.msec_timeout = timeout.value(); - return true; - } - - BT::NodeStatus onResultReceived(const WrappedResult& wr) override - { - RCLCPP_INFO( node_->get_logger(), "onResultReceived %d", wr.result->done ); - return wr.result->done ? NodeStatus::SUCCESS : NodeStatus::FAILURE; - } - - virtual BT::NodeStatus onFailure(ActionNodeErrorCode error) override - { - RCLCPP_ERROR( node_->get_logger(), "onFailure %d", error ); - return NodeStatus::FAILURE; - } -}; - - -//----------------------------------------------------- - - // Simple tree, used to execute once each action. - static const char* xml_text = R"( - - - - - - - - - - - - - - - - )"; - -int main(int argc, char **argv) -{ - rclcpp::init(argc, argv); - auto nh = std::make_shared("sleep_client"); - - BehaviorTreeFactory factory; - - factory.registerNodeType("PrintValue"); - - ActionNodeParams params = {nh, "sleep_service", std::chrono::milliseconds(2000)}; - RegisterRosAction(factory, "Sleep", params); - - auto tree = factory.createTreeFromText(xml_text); - - NodeStatus status = NodeStatus::IDLE; - - while( rclcpp::ok() ) - { - status = tree.tickRoot(); - tree.sleep(std::chrono::milliseconds(100)); - } - - return 0; -} diff --git a/test/test_dummy_tree_node.hpp b/test/test_dummy_tree_node.hpp new file mode 100644 index 0000000..1f57454 --- /dev/null +++ b/test/test_dummy_tree_node.hpp @@ -0,0 +1,59 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Sarthak Mittal +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_DUMMY_TREE_NODE_HPP_ +#define TEST_DUMMY_TREE_NODE_HPP_ + +#include +#include + +namespace nav2_behavior_tree +{ + +/** + * @brief A Dummy TreeNode to be used as a child for testing nodes + * Returns the current status on tick without any execution logic + */ +class DummyNode : public BT::ActionNodeBase +{ +public: + DummyNode() + : BT::ActionNodeBase("dummy", {}) + { + } + + void changeStatus(BT::NodeStatus status) + { + setStatus(status); + } + + BT::NodeStatus executeTick() override + { + return tick(); + } + + BT::NodeStatus tick() override + { + return status(); + } + + void halt() override + { + } +}; + +} // namespace nav2_behavior_tree + +#endif // TEST_DUMMY_TREE_NODE_HPP_ diff --git a/test/test_service.hpp b/test/test_service.hpp new file mode 100644 index 0000000..1544783 --- /dev/null +++ b/test/test_service.hpp @@ -0,0 +1,60 @@ +// Copyright (c) 2020 Sarthak Mittal +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_SERVICE_HPP_ +#define TEST_SERVICE_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" + +template +class TestService : public rclcpp::Node +{ +public: + explicit TestService( + std::string service_name, + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) + : Node("test_service", options) + { + using namespace std::placeholders; // NOLINT + + server_ = create_service( + service_name, + std::bind(&TestService::handle_service, this, _1, _2, _3)); + } + + std::shared_ptr getCurrentRequest() const + { + return current_request_; + } + +protected: + virtual void handle_service( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) + { + (void)request_header; + (void)response; + current_request_ = request; + } + +private: + typename rclcpp::Service::SharedPtr server_; + std::shared_ptr current_request_; +}; + +#endif // TEST_SERVICE_HPP_ diff --git a/test/test_transform_handler.hpp b/test/test_transform_handler.hpp new file mode 100644 index 0000000..dafb98e --- /dev/null +++ b/test/test_transform_handler.hpp @@ -0,0 +1,165 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Sarthak Mittal +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_TRANSFORM_HANDLER_HPP_ +#define TEST_TRANSFORM_HANDLER_HPP_ + +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "nav2_util/node_thread.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "tf2_msgs/msg/tf_message.hpp" +#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/transform_listener.h" +#include "tf2_ros/buffer.h" + +using namespace std::chrono_literals; // NOLINT +using namespace std::chrono; // NOLINT + +namespace nav2_behavior_tree +{ +class TransformHandler +{ +public: + explicit TransformHandler(rclcpp::Node::SharedPtr & node) + : node_(node), + is_active_(false), + base_transform_(nullptr), + tf_broadcaster_(nullptr) + { + tf_buffer_ = std::make_shared(node_->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + } + + ~TransformHandler() + { + if (is_active_) { + deactivate(); + } + } + + // Activate the tester before running tests + void activate() + { + if (is_active_) { + throw std::runtime_error("Trying to activate while already active"); + } + is_active_ = true; + + // Launch a thread to process the messages for this node + spin_thread_ = std::make_unique(node_->get_node_base_interface()); + + startRobotTransform(); + } + + void deactivate() + { + if (!is_active_) { + throw std::runtime_error("Trying to deactivate while already inactive"); + } + is_active_ = false; + spin_thread_.reset(); + tf_broadcaster_.reset(); + tf_buffer_.reset(); + tf_listener_.reset(); + } + + std::shared_ptr getBuffer() + { + return tf_buffer_; + } + + void waitForTransform() + { + if (is_active_) { + while (!tf_buffer_->canTransform("map", "base_link", rclcpp::Time(0))) { + std::this_thread::sleep_for(100ms); + } + RCLCPP_INFO(node_->get_logger(), "Transforms are available now!"); + return; + } + throw std::runtime_error("Trying to deactivate while already inactive"); + } + + void updateRobotPose(const geometry_msgs::msg::Pose & pose) + { + // Update base transform to publish + base_transform_->transform.translation.x = pose.position.x; + base_transform_->transform.translation.y = pose.position.y; + base_transform_->transform.translation.z = pose.position.z; + base_transform_->transform.rotation.x = pose.orientation.x; + base_transform_->transform.rotation.y = pose.orientation.y; + base_transform_->transform.rotation.z = pose.orientation.z; + base_transform_->transform.rotation.w = pose.orientation.w; + publishRobotTransform(); + } + +private: + void publishRobotTransform() + { + base_transform_->header.stamp = node_->now(); + tf_broadcaster_->sendTransform(*base_transform_); + } + + void startRobotTransform() + { + // Provide the robot pose transform + tf_broadcaster_ = std::make_shared(node_); + + if (!base_transform_) { + base_transform_ = std::make_unique(); + base_transform_->header.frame_id = "map"; + base_transform_->child_frame_id = "base_link"; + } + + // Set an initial pose + geometry_msgs::msg::Pose robot_pose; + robot_pose.position.x = 0; + robot_pose.position.y = 0; + robot_pose.orientation.w = 1; + updateRobotPose(robot_pose); + + // Publish the transform periodically + transform_timer_ = node_->create_wall_timer( + 100ms, std::bind(&TransformHandler::publishRobotTransform, this)); + } + + rclcpp::Node::SharedPtr node_; + + bool is_active_; + + // A thread for spinning the ROS node + std::unique_ptr spin_thread_; + + // Subscriber + + // The tester must provide the robot pose through a transform + std::unique_ptr base_transform_; + std::shared_ptr tf_broadcaster_; + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + rclcpp::TimerBase::SharedPtr transform_timer_; +}; + +} // namespace nav2_behavior_tree + +#endif // TEST_TRANSFORM_HANDLER_HPP_ From 3a79b28f8c791adaec0d1985e269dd138680a0b7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Victor=20Massagu=C3=A9=20Respall?= Date: Thu, 1 Dec 2022 12:55:14 +0100 Subject: [PATCH 2/8] Add method to register BT action nodes --- include/behaviortree_ros2/bt_action_node.hpp | 21 +++++++++++++++ include/behaviortree_ros2/bt_service_node.hpp | 26 ++++++++++++++++--- 2 files changed, 44 insertions(+), 3 deletions(-) diff --git a/include/behaviortree_ros2/bt_action_node.hpp b/include/behaviortree_ros2/bt_action_node.hpp index 6e90f1d..29556e0 100644 --- a/include/behaviortree_ros2/bt_action_node.hpp +++ b/include/behaviortree_ros2/bt_action_node.hpp @@ -21,6 +21,7 @@ #include #include "behaviortree_cpp/action_node.h" +#include "behaviortree_cpp/bt_factory.h" #include "behaviortree_ros2/bt_conversions.hpp" namespace BT @@ -448,6 +449,26 @@ class BtActionNode : public ActionNodeBase rclcpp::Time time_goal_sent_; }; +/// Method to register the bt action into a factory. +template static + void register_bt_action(BehaviorTreeFactory& factory, + const std::string& registration_ID, + const std::string& action_name) +{ + NodeBuilder builder = [=](const std::string& name, const NodeConfig& config) + { + return std::make_unique(name, action_name, config); + }; + + TreeNodeManifest manifest; + manifest.type = getType(); + manifest.ports = DerivedT::providedPorts(); + manifest.registration_ID = registration_ID; + const auto& basic_ports = DerivedT::providedPorts(); + manifest.ports.insert(basic_ports.begin(), basic_ports.end()); + factory.registerBuilder(manifest, builder); +} + } // namespace BT #endif // BEHAVIOR_TREE_ROS2__BT_ACTION_NODE_HPP_ diff --git a/include/behaviortree_ros2/bt_service_node.hpp b/include/behaviortree_ros2/bt_service_node.hpp index 0f516e0..c3cad64 100644 --- a/include/behaviortree_ros2/bt_service_node.hpp +++ b/include/behaviortree_ros2/bt_service_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_TREE_ROS2__BT_ACTION_NODE_HPP_ -#define BEHAVIOR_TREE_ROS2__BT_ACTION_NODE_HPP_ +#ifndef BEHAVIOR_TREE_ROS2__BT_SERVICE_NODE_HPP_ +#define BEHAVIOR_TREE_ROS2__BT_SERVICE_NODE_HPP_ #include #include @@ -236,6 +236,26 @@ class BtServiceNode : public ActionNodeBase rclcpp::Time sent_time_; }; +/// Method to register the bt action into a factory. +// template static +// void register_bt_action(BT::BehaviorTreeFactory& factory, +// const std::string& registration_ID, +// const std::string& action_name) +// { +// NodeBuilder builder = [=](const std::string& name, const NodeConfig& config) +// { +// return std::make_unique(name, action_name, config); +// }; + +// TreeNodeManifest manifest; +// manifest.type = getType(); +// manifest.ports = DerivedT::providedPorts(); +// manifest.registration_ID = registration_ID; +// const auto& basic_ports = DerivedT::providedPorts(); +// manifest.ports.insert(basic_ports.begin(), basic_ports.end()); +// factory.registerBuilder(manifest, builder); +// } + } // namespace BT -#endif // BEHAVIOR_TREE_ROS2__BT_ACTION_NODE_HPP_ +#endif // BEHAVIOR_TREE_ROS2__BT_SERVICE_NODE_HPP_ From b8267ab6a6e669bc325fdfc39d1c4163b273df9d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Victor=20Massagu=C3=A9=20Respall?= Date: Thu, 1 Dec 2022 16:23:09 +0100 Subject: [PATCH 3/8] Replace setStatus IDLE with resetStatus --- include/behaviortree_ros2/bt_action_node.hpp | 2 +- include/behaviortree_ros2/bt_service_node.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/include/behaviortree_ros2/bt_action_node.hpp b/include/behaviortree_ros2/bt_action_node.hpp index 29556e0..2f3afb3 100644 --- a/include/behaviortree_ros2/bt_action_node.hpp +++ b/include/behaviortree_ros2/bt_action_node.hpp @@ -304,7 +304,7 @@ class BtActionNode : public ActionNodeBase } } - setStatus(NodeStatus::IDLE); + resetStatus(); } protected: diff --git a/include/behaviortree_ros2/bt_service_node.hpp b/include/behaviortree_ros2/bt_service_node.hpp index c3cad64..bc1cfa9 100644 --- a/include/behaviortree_ros2/bt_service_node.hpp +++ b/include/behaviortree_ros2/bt_service_node.hpp @@ -136,7 +136,7 @@ class BtServiceNode : public ActionNodeBase void halt() override { request_sent_ = false; - setStatus(NodeStatus::IDLE); + resetStatus(); } /** From 2be58345815ba17c22e6a9e8d1432e76bd656a4d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Victor=20Massagu=C3=A9=20Respall?= Date: Fri, 2 Dec 2022 09:20:58 +0100 Subject: [PATCH 4/8] Add PipelineSequence control node --- CMakeLists.txt | 10 ++ .../plugins/control/pipeline_sequence.hpp | 93 +++++++++++++++++++ plugins/control/pipeline_sequence.cpp | 80 ++++++++++++++++ 3 files changed, 183 insertions(+) create mode 100644 include/behaviortree_ros2/plugins/control/pipeline_sequence.hpp create mode 100644 plugins/control/pipeline_sequence.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 6a0df0d..b0851bf 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -36,8 +36,17 @@ ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS} ) +add_library(nav2_pipeline_sequence_bt_node SHARED plugins/control/pipeline_sequence.cpp) +list(APPEND plugin_libs nav2_pipeline_sequence_bt_node) + +foreach(bt_plugin ${plugin_libs}) + ament_target_dependencies(${bt_plugin} ${dependencies}) + target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT) +endforeach() + install(TARGETS ${PROJECT_NAME} + ${plugin_libs} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -53,6 +62,7 @@ ament_export_include_directories( ament_export_libraries( ${PROJECT_NAME} + ${plugin_libs} ) ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) diff --git a/include/behaviortree_ros2/plugins/control/pipeline_sequence.hpp b/include/behaviortree_ros2/plugins/control/pipeline_sequence.hpp new file mode 100644 index 0000000..0bddc22 --- /dev/null +++ b/include/behaviortree_ros2/plugins/control/pipeline_sequence.hpp @@ -0,0 +1,93 @@ +// Copyright (c) 2019 Intel Corporation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BEHAVIORTREE_ROS2__PLUGINS__CONTROL__PIPELINE_SEQUENCE_HPP_ +#define BEHAVIORTREE_ROS2__PLUGINS__CONTROL__PIPELINE_SEQUENCE_HPP_ + +#include +#include "behaviortree_cpp/control_node.h" +#include "behaviortree_cpp/bt_factory.h" + +namespace BT +{ + +/** @brief Type of sequence node that re-ticks previous children when a child returns running + * + * Type of Control Node | Child Returns Failure | Child Returns Running + * --------------------------------------------------------------------- + * PipelineSequence | Restart | Tick All Previous Again + * + * Tick All Previous Again means every node up till this one will be reticked. Even + * if a previous node returns Running, the next node will be reticked. + * + * As an example, let's say this node has 3 children: A, B and C. At the start, + * they are all IDLE. + * | A | B | C | + * -------------------------------- + * | IDLE | IDLE | IDLE | + * | RUNNING | IDLE | IDLE | - at first A gets ticked. Assume it returns RUNNING + * - PipelineSequence returns RUNNING and no other nodes are ticked. + * | SUCCESS | RUNNING | IDLE | - This time A returns SUCCESS so B gets ticked as well + * - PipelineSequence returns RUNNING and C is not ticked yet + * | RUNNING | SUCCESS | RUNNING | - A gets ticked and returns RUNNING, but since it had previously + * - returned SUCCESS, PipelineSequence continues on and ticks B. + * - Since B also returns SUCCESS, C gets ticked this time as well. + * | RUNNING | SUCCESS | SUCCESS | - A is still RUNNING, and B returns SUCCESS again. This time C + * - returned SUCCESS, ending the sequence. PipelineSequence + * - returns SUCCESS and halts A. + * + * If any children at any time had returned FAILURE. PipelineSequence would have returned FAILURE + * and halted all children, ending the sequence. + * + * Usage in XML: + */ +class PipelineSequence : public ControlNode +{ +public: + /** + * @brief A constructor for nav2_behavior_tree::PipelineSequence + * @param name Name for the XML tag for this node + */ + explicit PipelineSequence(const std::string & name); + + /** + * @brief A constructor for nav2_behavior_tree::PipelineSequence + * @param name Name for the XML tag for this node + * @param config BT node configuration + */ + PipelineSequence(const std::string & name, const NodeConfig & config); + + /** + * @brief The other (optional) override required by a BT action to reset node state + */ + void halt() override; + + /** + * @brief Creates list of BT ports + * @return PortsList Containing basic ports along with node-specific ports + */ + static PortsList providedPorts() {return {};} + +protected: + /** + * @brief The main override required by a BT action + * @return NodeStatus Status of tick execution + */ + NodeStatus tick() override; + + std::size_t last_child_ticked_ = 0; +}; +} // namespace BT + +#endif // BEHAVIORTREE_ROS2__PLUGINS__CONTROL__PIPELINE_SEQUENCE_HPP_ diff --git a/plugins/control/pipeline_sequence.cpp b/plugins/control/pipeline_sequence.cpp new file mode 100644 index 0000000..1562812 --- /dev/null +++ b/plugins/control/pipeline_sequence.cpp @@ -0,0 +1,80 @@ +// Copyright (c) 2019 Intel Corporation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include + +#include "behaviortree_ros2/plugins/control/pipeline_sequence.hpp" + +namespace BT +{ + +PipelineSequence::PipelineSequence(const std::string & name) +: BT::ControlNode(name, {}) +{ +} + +PipelineSequence::PipelineSequence( + const std::string & name, + const BT::NodeConfig & config) +: BT::ControlNode(name, config) +{ +} + +BT::NodeStatus PipelineSequence::tick() +{ + for (std::size_t i = 0; i < children_nodes_.size(); ++i) { + auto status = children_nodes_[i]->executeTick(); + switch (status) { + case BT::NodeStatus::FAILURE: + ControlNode::haltChildren(); + last_child_ticked_ = 0; // reset + return status; + case BT::NodeStatus::SUCCESS: + // do nothing and continue on to the next child. If it is the last child + // we'll exit the loop and hit the wrap-up code at the end of the method. + break; + case BT::NodeStatus::RUNNING: + if (i >= last_child_ticked_) { + last_child_ticked_ = i; + return status; + } + // else do nothing and continue on to the next child + break; + default: + std::stringstream error_msg; + error_msg << "Invalid node status. Received status " << status << + "from child " << children_nodes_[i]->name(); + throw std::runtime_error(error_msg.str()); + } + } + // Wrap up. + ControlNode::haltChildren(); + last_child_ticked_ = 0; // reset + return BT::NodeStatus::SUCCESS; +} + +void PipelineSequence::halt() +{ + BT::ControlNode::halt(); + last_child_ticked_ = 0; +} + +} // namespace BT + +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("PipelineSequence"); +} From ee14d63c29d8be4256cd4245c9c319ea0ee9f62b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Victor=20Massagu=C3=A9=20Respall?= Date: Fri, 2 Dec 2022 09:48:55 +0100 Subject: [PATCH 5/8] Add PipelineSequence control node --- CMakeLists.txt | 2 +- plugins/control/pipeline_sequence.cpp | 20 ++++++++++---------- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b0851bf..4923aea 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -40,7 +40,7 @@ add_library(nav2_pipeline_sequence_bt_node SHARED plugins/control/pipeline_seque list(APPEND plugin_libs nav2_pipeline_sequence_bt_node) foreach(bt_plugin ${plugin_libs}) - ament_target_dependencies(${bt_plugin} ${dependencies}) + ament_target_dependencies(${bt_plugin} ${THIS_PACKAGE_INCLUDE_DEPENDS}) target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT) endforeach() diff --git a/plugins/control/pipeline_sequence.cpp b/plugins/control/pipeline_sequence.cpp index 1562812..ab4526a 100644 --- a/plugins/control/pipeline_sequence.cpp +++ b/plugins/control/pipeline_sequence.cpp @@ -22,31 +22,31 @@ namespace BT { PipelineSequence::PipelineSequence(const std::string & name) -: BT::ControlNode(name, {}) +: ControlNode(name, {}) { } PipelineSequence::PipelineSequence( const std::string & name, - const BT::NodeConfig & config) -: BT::ControlNode(name, config) + const NodeConfig & config) +: ControlNode(name, config) { } -BT::NodeStatus PipelineSequence::tick() +NodeStatus PipelineSequence::tick() { for (std::size_t i = 0; i < children_nodes_.size(); ++i) { auto status = children_nodes_[i]->executeTick(); switch (status) { - case BT::NodeStatus::FAILURE: + case NodeStatus::FAILURE: ControlNode::haltChildren(); last_child_ticked_ = 0; // reset return status; - case BT::NodeStatus::SUCCESS: + case NodeStatus::SUCCESS: // do nothing and continue on to the next child. If it is the last child // we'll exit the loop and hit the wrap-up code at the end of the method. break; - case BT::NodeStatus::RUNNING: + case NodeStatus::RUNNING: if (i >= last_child_ticked_) { last_child_ticked_ = i; return status; @@ -63,12 +63,12 @@ BT::NodeStatus PipelineSequence::tick() // Wrap up. ControlNode::haltChildren(); last_child_ticked_ = 0; // reset - return BT::NodeStatus::SUCCESS; + return NodeStatus::SUCCESS; } void PipelineSequence::halt() { - BT::ControlNode::halt(); + ControlNode::halt(); last_child_ticked_ = 0; } @@ -76,5 +76,5 @@ void PipelineSequence::halt() BT_REGISTER_NODES(factory) { - factory.registerNodeType("PipelineSequence"); + factory.registerNodeType("PipelineSequence"); } From 0806dd2d7f48df01c97670a4815a46db3e4a1bfd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Victor=20Massagu=C3=A9=20Respall?= Date: Fri, 2 Dec 2022 14:33:21 +0100 Subject: [PATCH 6/8] Add resgister node for BT service nodes --- CMakeLists.txt | 4 +- include/behaviortree_ros2/bt_service_node.hpp | 54 ++++++++++--------- 2 files changed, 32 insertions(+), 26 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 4923aea..9131331 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -36,8 +36,8 @@ ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS} ) -add_library(nav2_pipeline_sequence_bt_node SHARED plugins/control/pipeline_sequence.cpp) -list(APPEND plugin_libs nav2_pipeline_sequence_bt_node) +add_library(pipeline_sequence_bt_node SHARED plugins/control/pipeline_sequence.cpp) +list(APPEND plugin_libs pipeline_sequence_bt_node) foreach(bt_plugin ${plugin_libs}) ament_target_dependencies(${bt_plugin} ${THIS_PACKAGE_INCLUDE_DEPENDS}) diff --git a/include/behaviortree_ros2/bt_service_node.hpp b/include/behaviortree_ros2/bt_service_node.hpp index bc1cfa9..2b84b3a 100644 --- a/include/behaviortree_ros2/bt_service_node.hpp +++ b/include/behaviortree_ros2/bt_service_node.hpp @@ -35,13 +35,15 @@ class BtServiceNode : public ActionNodeBase public: /** * @brief A nav2_behavior_tree::BtServiceNode constructor + * @param xml_tag_name Name for the XML tag for this node * @param service_node_name Service name this node creates a client for * @param conf BT node configuration */ BtServiceNode( - const std::string & service_node_name, + const std::string & xml_tag_name, + const std::string & service_name, const NodeConfig& conf) - : ActionNodeBase(service_node_name, conf), service_node_name_(service_node_name) + : ActionNodeBase(xml_tag_name, conf), service_name_(service_name) { node_ = config().blackboard->template get("node"); callback_group_ = node_->create_callback_group( @@ -57,7 +59,11 @@ class BtServiceNode : public ActionNodeBase getInput("server_timeout", server_timeout_); // Now that we have node_ to use, create the service client for this BT service - getInput("service_name", service_name_); + std::string remapped_service_name; + if(getInput("service_name", remapped_service_name)) + { + service_name_ = remapped_service_name; + } std::string node_namespace; node_namespace = node_->get_namespace(); // Append namespace to the action name @@ -80,7 +86,7 @@ class BtServiceNode : public ActionNodeBase RCLCPP_DEBUG( node_->get_logger(), "\"%s\" BtServiceNode initialized", - service_node_name_.c_str()); + service_name_.c_str()); } BtServiceNode() = delete; @@ -214,7 +220,7 @@ class BtServiceNode : public ActionNodeBase config().blackboard->template set("number_recoveries", recovery_count); // NOLINT } - std::string service_name_, service_node_name_; + std::string service_name_; typename std::shared_ptr> service_client_; std::shared_ptr request_; @@ -236,25 +242,25 @@ class BtServiceNode : public ActionNodeBase rclcpp::Time sent_time_; }; -/// Method to register the bt action into a factory. -// template static -// void register_bt_action(BT::BehaviorTreeFactory& factory, -// const std::string& registration_ID, -// const std::string& action_name) -// { -// NodeBuilder builder = [=](const std::string& name, const NodeConfig& config) -// { -// return std::make_unique(name, action_name, config); -// }; - -// TreeNodeManifest manifest; -// manifest.type = getType(); -// manifest.ports = DerivedT::providedPorts(); -// manifest.registration_ID = registration_ID; -// const auto& basic_ports = DerivedT::providedPorts(); -// manifest.ports.insert(basic_ports.begin(), basic_ports.end()); -// factory.registerBuilder(manifest, builder); -// } +/ Method to register the bt action into a factory. +template static +void register_bt_action(BT::BehaviorTreeFactory& factory, + const std::string& registration_ID, + const std::string& service_name) +{ + NodeBuilder builder = [=](const std::string& name, const NodeConfig& config) + { + return std::make_unique(name, service_name, config); + }; + + TreeNodeManifest manifest; + manifest.type = getType(); + manifest.ports = DerivedT::providedPorts(); + manifest.registration_ID = registration_ID; + const auto& basic_ports = DerivedT::providedPorts(); + manifest.ports.insert(basic_ports.begin(), basic_ports.end()); + factory.registerBuilder(manifest, builder); +} } // namespace BT From 3e2d62adfc6f4d7551d6dff616963b4233eb65f6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Victor=20Massagu=C3=A9=20Respall?= Date: Fri, 2 Dec 2022 16:32:40 +0100 Subject: [PATCH 7/8] Fix typo with the register bt action method --- include/behaviortree_ros2/bt_service_node.hpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/include/behaviortree_ros2/bt_service_node.hpp b/include/behaviortree_ros2/bt_service_node.hpp index 2b84b3a..235c66b 100644 --- a/include/behaviortree_ros2/bt_service_node.hpp +++ b/include/behaviortree_ros2/bt_service_node.hpp @@ -20,6 +20,7 @@ #include #include "behaviortree_cpp/action_node.h" +#include "behaviortree_cpp/bt_factory.h" #include "behaviortree_ros2/bt_conversions.hpp" namespace BT @@ -242,9 +243,9 @@ class BtServiceNode : public ActionNodeBase rclcpp::Time sent_time_; }; -/ Method to register the bt action into a factory. +/// Method to register the bt action into a factory. template static -void register_bt_action(BT::BehaviorTreeFactory& factory, + void register_bt_action(BehaviorTreeFactory& factory, const std::string& registration_ID, const std::string& service_name) { From d963d36339b0ed2f25ff7454d99ac274e6803932 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Victor=20Massagu=C3=A9=20Respall?= Date: Tue, 13 Dec 2022 11:46:03 +0100 Subject: [PATCH 8/8] Add rate_controller decorator --- CMakeLists.txt | 3 + .../plugins/decorator/rate_controller.hpp | 66 ++++++++++++++ plugins/decorator/rate_controller.cpp | 85 +++++++++++++++++++ 3 files changed, 154 insertions(+) create mode 100644 include/behaviortree_ros2/plugins/decorator/rate_controller.hpp create mode 100644 plugins/decorator/rate_controller.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 9131331..8bfffc2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -39,6 +39,9 @@ ament_target_dependencies(${PROJECT_NAME} add_library(pipeline_sequence_bt_node SHARED plugins/control/pipeline_sequence.cpp) list(APPEND plugin_libs pipeline_sequence_bt_node) +add_library(rate_controller_bt_node SHARED plugins/decorator/rate_controller.cpp) +list(APPEND plugin_libs rate_controller_bt_node) + foreach(bt_plugin ${plugin_libs}) ament_target_dependencies(${bt_plugin} ${THIS_PACKAGE_INCLUDE_DEPENDS}) target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT) diff --git a/include/behaviortree_ros2/plugins/decorator/rate_controller.hpp b/include/behaviortree_ros2/plugins/decorator/rate_controller.hpp new file mode 100644 index 0000000..30a9478 --- /dev/null +++ b/include/behaviortree_ros2/plugins/decorator/rate_controller.hpp @@ -0,0 +1,66 @@ +// Copyright (c) 2018 Intel Corporation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BEHAVIORTREE_ROS2__PLUGINS__DECORATOR__RATE_CONTROLLER_HPP_ +#define BEHAVIORTREE_ROS2__PLUGINS__DECORATOR__RATE_CONTROLLER_HPP_ + +#include +#include + +#include "behaviortree_cpp/decorator_node.h" + +namespace BT +{ + +/** + * @brief A DecoratorNode that ticks its child at a specified rate + */ +class RateController : public DecoratorNode +{ +public: + /** + * @brief A constructor for RateController + * @param name Name for the XML tag for this node + * @param conf BT node configuration + */ + RateController( + const std::string & name, + const NodeConfig & conf); + + /** + * @brief Creates list of BT ports + * @return PortsList Containing node-specific ports + */ + static PortsList providedPorts() + { + return { + InputPort("hz", 10.0, "Rate") + }; + } + +private: + /** + * @brief The main override required by a BT action + * @return NodeStatus Status of tick execution + */ + NodeStatus tick() override; + + std::chrono::time_point start_; + double period_; + bool first_time_; +}; + +} // namespace BT + +#endif // BEHAVIORTREE_ROS2__PLUGINS__DECORATOR__RATE_CONTROLLER_HPP_ diff --git a/plugins/decorator/rate_controller.cpp b/plugins/decorator/rate_controller.cpp new file mode 100644 index 0000000..6d4ba5a --- /dev/null +++ b/plugins/decorator/rate_controller.cpp @@ -0,0 +1,85 @@ +// Copyright (c) 2018 Intel Corporation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "behaviortree_ros2/plugins/decorator/rate_controller.hpp" + +namespace BT +{ + +RateController::RateController( + const std::string & name, + const NodeConfig & conf) +: DecoratorNode(name, conf), + first_time_(false) +{ + double hz = 1.0; + getInput("hz", hz); + period_ = 1.0 / hz; +} + +NodeStatus RateController::tick() +{ + if (status() == NodeStatus::IDLE) { + // Reset the starting point since we're starting a new iteration of + // the rate controller (moving from IDLE to RUNNING) + start_ = std::chrono::high_resolution_clock::now(); + first_time_ = true; + } + + setStatus(NodeStatus::RUNNING); + + // Determine how long its been since we've started this iteration + auto now = std::chrono::high_resolution_clock::now(); + auto elapsed = now - start_; + + // Now, get that in seconds + typedef std::chrono::duration float_seconds; + auto seconds = std::chrono::duration_cast(elapsed); + + // The child gets ticked the first time through and any time the period has + // expired. In addition, once the child begins to run, it is ticked each time + // 'til completion + if (first_time_ || (child_node_->status() == NodeStatus::RUNNING) || + seconds.count() >= period_) + { + first_time_ = false; + const NodeStatus child_state = child_node_->executeTick(); + + switch (child_state) { + case NodeStatus::RUNNING: + return NodeStatus::RUNNING; + + case NodeStatus::SUCCESS: + start_ = std::chrono::high_resolution_clock::now(); // Reset the timer + return NodeStatus::SUCCESS; + + case NodeStatus::FAILURE: + default: + return NodeStatus::FAILURE; + } + } + + return status(); +} + +} // namespace BT + +#include +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("RateController"); +}