diff --git a/CMakeLists.txt b/CMakeLists.txt index 9805048..7d5f938 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -44,8 +44,8 @@ generate_messages( ) catkin_package( - INCLUDE_DIRS include - LIBRARIES + INCLUDE_DIRS include include/behaviortree_ros + LIBRARIES behaviortree_ros CATKIN_DEPENDS ${ROS_DEPENDENCIES} ) @@ -56,8 +56,11 @@ include_directories( include ${catkin_INCLUDE_DIRS}) #header only for the time being -#add_library(behaviortree_ros ... ) -#target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) +add_library(behaviortree_ros src/loggers/rosout_logger.cpp src/loggers/ros_topic_logger.cpp src/recovery_node.cpp) +target_link_libraries(behaviortree_ros ${catkin_LIBRARIES}) +target_compile_definitions(behaviortree_ros PRIVATE BT_PLUGIN_EXPORT ) + +add_dependencies(behaviortree_ros behaviortree_ros_gencpp) ###################################################### # TESTS @@ -73,8 +76,14 @@ target_link_libraries(test_server ${catkin_LIBRARIES} ) ###################################################### # INSTALL -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) +install(TARGETS behaviortree_ros +ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) +install ( + DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h") ###################################################### # EXAMPLES and TOOLS diff --git a/include/behaviortree_ros/bt_async_service_node.h b/include/behaviortree_ros/bt_async_service_node.h new file mode 100644 index 0000000..575d967 --- /dev/null +++ b/include/behaviortree_ros/bt_async_service_node.h @@ -0,0 +1,179 @@ +// Copyright (c) 2019 Samsung Research America +// Copyright (c) 2020 Davide Faconti +// +// 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_BT_SERVICE_NODE_HPP_ +#define BEHAVIOR_TREE_BT_SERVICE_NODE_HPP_ + +#include +#include +#include +#include + +namespace BT +{ + +/** + * Base Action to implement a ROS Service + */ +template +class RosAsyncServiceNode : public BT::ActionNodeBase +{ +protected: + + RosAsyncServiceNode(ros::NodeHandle& nh, const std::string& name, const BT::NodeConfiguration & conf): + BT::ActionNodeBase(name, conf), node_(nh) { + } + + RosAsyncServiceNode(ros::NodeHandle& nh,const std::string& service_name, ros::Duration timeout, const std::string& name, const BT::NodeConfiguration & conf): + BT::ActionNodeBase(name, conf), node_(nh) ,service_name_(service_name), timeout_(timeout) { + std::cout << "RosAsyncServiceNode constructor" << std::endl; + std::cout << "service_name: " << service_name << std::endl; + service_client_ = node_.serviceClient( service_name_ ); + } + +public: + + using BaseClass = RosAsyncServiceNode; + using ServiceType = ServiceT; + using RequestType = typename ServiceT::Request; + using ResponseType = typename ServiceT::Response; + + RosAsyncServiceNode() = delete; + + virtual ~RosAsyncServiceNode() = default; + + /// These ports will be added automatically if this Node is + /// registered using RegisterRosAction() + static PortsList providedPorts() + { + return { + InputPort("service_name", "name of the ROS service"), + InputPort("timeout", 100, "timeout to connect to server (milliseconds)") + }; + } + + /// User must implement this method. + virtual void sendRequest(RequestType& request) = 0; + + /// Method (to be implemented by the user) to receive the reply. + /// User can decide which NodeStatus it will return (SUCCESS or FAILURE). + virtual NodeStatus onResponse( const ResponseType& rep) = 0; + + enum FailureCause{ + MISSING_SERVER = 0, + FAILED_CALL = 1 + }; + + /// Called when a service call failed. Can be overriden by the user. + virtual NodeStatus onFailedRequest(FailureCause failure) + { + return NodeStatus::FAILURE; + } + +protected: + + ros::ServiceClient service_client_; + std::string service_name_; + + RequestType request_; + ResponseType reply_; + + + + // The node that will be used for any ROS operations + ros::NodeHandle& node_; + + ros::Duration timeout_; + + std::future< std::pair > future_; + + std::pair call_service(RequestType &request, ros::Duration timeout){ + bool connected = service_client_.waitForExistence(timeout); + if( !connected ){ + return std::make_pair(false, false); + } + bool received = service_client_.call( request, reply_ ); + return std::make_pair(true, received); + + } + + BT::NodeStatus tick() override + { + if( !service_client_.isValid() ){ + + service_client_ = node_.serviceClient( service_name_ ); + } + + // first step to be done only at the beginning of the Action + if (status() == BT::NodeStatus::IDLE) { + // setting the status to RUNNING to notify the BT Loggers (if any) + setStatus(BT::NodeStatus::RUNNING); + + sendRequest(request_); + + future_ = std::async(std::launch::async, &RosAsyncServiceNode::call_service, this, std::ref(request_), timeout_); + + } + + if( future_.valid() ){ + if (future_.wait_for(std::chrono::seconds(0)) != std::future_status::ready) { + return BT::NodeStatus::RUNNING; + } + auto result = future_.get(); + if( !result.first ){ + return onFailedRequest(MISSING_SERVER); + } + if( !result.second ){ + return onFailedRequest(FAILED_CALL); + } + return onResponse(reply_); + } + throw BT::LogicError("future is not valid, this should never happen"); + return BT::NodeStatus::RUNNING; + } + + //halt just resets the future so that result will be ignored, and next tick will start a new future + virtual void halt() override{ + future_ = std::future< std::pair >(); + } + +}; + + +/// Method to register the service into a factory. +/// It gives you the opportunity to set the ros::NodeHandle. +template static + void RegisterRosAsyncService(BT::BehaviorTreeFactory& factory, + const std::string& registration_ID, + ros::NodeHandle& node_handle, std::string service_name, ros::Duration timeout) +{ + NodeBuilder builder = [&node_handle, service_name , timeout](const std::string& name, const NodeConfiguration& config) { + return std::make_unique(node_handle, service_name, timeout, name, config ); + }; + + TreeNodeManifest manifest; + manifest.type = getType(); + manifest.ports = DerivedT::providedPorts(); + manifest.registration_ID = registration_ID; + //const auto& basic_ports = RosAsyncServiceNode< typename DerivedT::ServiceType>::providedPorts(); + //manifest.ports.insert( basic_ports.begin(), basic_ports.end() ); + + factory.registerBuilder( manifest, builder ); +} + + +} // namespace BT + +#endif // BEHAVIOR_TREE_BT_SERVICE_NODE_HPP_ diff --git a/include/behaviortree_ros/loggers/ros_topic_logger.h b/include/behaviortree_ros/loggers/ros_topic_logger.h new file mode 100644 index 0000000..02d1194 --- /dev/null +++ b/include/behaviortree_ros/loggers/ros_topic_logger.h @@ -0,0 +1,36 @@ +#pragma once + +#include +#include +#include +#include + + +namespace BT +{ + +class RosTopicLogger : public StatusChangeLogger +{ + static std::atomic ref_count; + + public: + RosTopicLogger(TreeNode* root_node, ros::NodeHandle nh, const std::string topic_name = "behavior_tree_log"); + + + ~RosTopicLogger() override; + + virtual void callback(Duration timestamp, + const TreeNode& node, + NodeStatus prev_status, + NodeStatus status) override; + + virtual void flush() override; + +private: + ros::NodeHandle nh_; + std::string topic_name_; + ros::Publisher status_change_pub_; + std::vector event_log_; +}; + +} // end namespace \ No newline at end of file diff --git a/include/behaviortree_ros/recovery_node.h b/include/behaviortree_ros/recovery_node.h new file mode 100644 index 0000000..f8f529c --- /dev/null +++ b/include/behaviortree_ros/recovery_node.h @@ -0,0 +1,79 @@ +// 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. + +#pragma once + +#include +#include "behaviortree_cpp_v3/control_node.h" + +namespace behaviortree_ros +{ +/** + * @brief The RecoveryNode has only two children and returns SUCCESS if and only if the first child + * returns SUCCESS. + * + * - If the first child returns FAILURE, the second child will be executed. After that the first + * child is executed again if the second child returns SUCCESS. + * + * - If the first or second child returns RUNNING, this node returns RUNNING. + * + * - If the second child returns FAILURE, this control node will stop the loop and returns FAILURE. + * + */ +class RecoveryNode : public BT::ControlNode +{ +public: + /** + * @brief A constructor for nav2_behavior_tree::RecoveryNode + * @param name Name for the XML tag for this node + * @param conf BT node configuration + */ + RecoveryNode( + const std::string & name, + const BT::NodeConfiguration & conf); + + /** + * @brief A destructor for nav2_behavior_tree::RecoveryNode + */ + ~RecoveryNode() override = default; + + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing basic ports along with node-specific ports + */ + static BT::PortsList providedPorts() + { + return { + BT::InputPort("number_of_retries", 1, "Number of retries") + }; + } + +private: + unsigned int current_child_idx_; + unsigned int number_of_retries_; + unsigned int retry_count_; + + /** + * @brief The main override required by a BT action + * @return BT::NodeStatus Status of tick execution + */ + BT::NodeStatus tick() override; + + /** + * @brief The other (optional) override required by a BT action to reset node state + */ + void halt() override; +}; + +} // namespace behaviortree_ros diff --git a/include/behaviortree_ros/utils.h b/include/behaviortree_ros/utils.h new file mode 100644 index 0000000..2b075f1 --- /dev/null +++ b/include/behaviortree_ros/utils.h @@ -0,0 +1,61 @@ +#pragma once + +#include +#include +#include +#include +#include +namespace BT +{ + static behaviortree_ros::TreeNode convertToRosMsg(const TreeNode *node, int parent_id ) + { + behaviortree_ros::TreeNode msg; + + msg.uid = node->UID(); + msg.instance_name = node->name(); + msg.registration_name = node->registrationName(); + msg.type = static_cast(node->type()); + msg.parent_uid = parent_id; + msg.status.value = static_cast(node->status()); + + + return msg; + } + static void addNodesToMsg(const TreeNode *node, behaviortree_ros::BehaviorTree& msg, int parent_id ) + { + + msg.nodes.push_back(convertToRosMsg(node, parent_id)); + auto ¤t_msg = msg.nodes.back(); + + + if (auto control = dynamic_cast(node)) + { + for (const auto& child : control->children()) + { + addNodesToMsg(child, msg, node->UID()); + current_msg.children_uid.push_back(child->UID()); + } + } + else if (auto decorator = dynamic_cast(node)) + { + if (decorator->child()) + { + addNodesToMsg(decorator->child(), msg, node->UID()); + current_msg.children_uid.push_back(decorator->child()->UID()); + } + } + } + + static behaviortree_ros::BehaviorTree convertToRosMsg(const Tree& tree) + { + behaviortree_ros::BehaviorTree msg; + msg.nodes.reserve(tree.nodes.size()); + msg.root_uid = tree.rootNode()->UID(); + + addNodesToMsg(tree.rootNode(), msg, msg.root_uid); + return msg; + } + + + +} // namespace BT \ No newline at end of file diff --git a/msg/StatusChange.msg b/msg/StatusChange.msg index 1af72a5..66a2ea0 100644 --- a/msg/StatusChange.msg +++ b/msg/StatusChange.msg @@ -1,4 +1,5 @@ uint16 uid +string name NodeStatus prev_status NodeStatus status time timestamp diff --git a/msg/StatusChangeLog.msg b/msg/StatusChangeLog.msg index 594d592..1c48f23 100644 --- a/msg/StatusChangeLog.msg +++ b/msg/StatusChangeLog.msg @@ -1,3 +1,3 @@ -BehaviorTree behavior_tree + StatusChange[] state_changes diff --git a/msg/TreeNode.msg b/msg/TreeNode.msg index 8ea0958..ffcc106 100644 --- a/msg/TreeNode.msg +++ b/msg/TreeNode.msg @@ -1,4 +1,5 @@ uint16 uid +uint16 parent_uid uint16[] children_uid int8 type NodeStatus status diff --git a/package.xml b/package.xml index e97b070..317775b 100644 --- a/package.xml +++ b/package.xml @@ -16,16 +16,17 @@ behaviortree_cpp_v3 actionlib actionlib_msgs - + message_generation + roscpp std_msgs behaviortree_cpp_v3 actionlib actionlib_msgs - - message_generation + message_runtime message_generation + catkin diff --git a/src/loggers/ros_topic_logger.cpp b/src/loggers/ros_topic_logger.cpp new file mode 100644 index 0000000..7b653ae --- /dev/null +++ b/src/loggers/ros_topic_logger.cpp @@ -0,0 +1,59 @@ +#include "behaviortree_ros/loggers/ros_topic_logger.h" + +namespace BT +{ +std::atomic RosTopicLogger::ref_count(false); + +RosTopicLogger::RosTopicLogger(TreeNode* root_node, ros::NodeHandle nh, const std::string topic_name) : + StatusChangeLogger(root_node), + nh_(nh), + topic_name_(topic_name) +{ + bool expected = false; + if (!ref_count.compare_exchange_strong(expected, true)) + { + throw std::logic_error("Only a single instance of RosTopicLogger shall be created"); + } + status_change_pub_ = nh_.advertise(topic_name_, 5); + +} + + + +RosTopicLogger::~RosTopicLogger() +{ + ref_count.store(false); +} + +void RosTopicLogger::callback(Duration timestamp, const TreeNode& node, NodeStatus prev_status, + NodeStatus status) +{ + + behaviortree_ros::StatusChange event; + + // BT timestamps are a duration since the epoch. Need to convert to a time_point + // before converting to a msg. + + uint32_t sec = std::chrono::duration_cast(timestamp).count(); + auto remainder = timestamp - std::chrono::duration_cast(timestamp); + uint32_t nsec = std::chrono::duration_cast(remainder).count() ; + event.timestamp = ros::Time(sec, nsec); + event.uid = node.UID(); + event.name = node.name(); + event.prev_status.value = static_cast (prev_status); + event.status.value = static_cast (status); + event_log_.push_back(std::move(event)); + +} + +void RosTopicLogger::flush() +{ + if (!event_log_.empty()){ + behaviortree_ros::StatusChangeLog log_msg; + log_msg.state_changes = std::move(event_log_); + status_change_pub_.publish(log_msg); + event_log_.clear(); + } +} + +} // end namespace diff --git a/src/loggers/rosout_logger.cpp b/src/loggers/rosout_logger.cpp index af11584..bf22995 100644 --- a/src/loggers/rosout_logger.cpp +++ b/src/loggers/rosout_logger.cpp @@ -56,7 +56,7 @@ void RosoutLogger::callback(Duration timestamp, const TreeNode& node, NodeStatus ROS_INFO("[%s%s]: %s -> %s", node_name.c_str(), &whitespaces[std::min(ws_count, node_name.size())], toStr(prev_status, true).c_str(), - toStr(status, true).c_srt()); + toStr(status, true).c_str()); break; } } diff --git a/src/recovery_node.cpp b/src/recovery_node.cpp new file mode 100644 index 0000000..a749567 --- /dev/null +++ b/src/recovery_node.cpp @@ -0,0 +1,129 @@ +// 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 "behaviortree_ros/recovery_node.h" + +namespace behaviortree_ros +{ + +RecoveryNode::RecoveryNode( + const std::string & name, + const BT::NodeConfiguration & conf) +: BT::ControlNode::ControlNode(name, conf), + current_child_idx_(0), + number_of_retries_(1), + retry_count_(0) +{ + getInput("number_of_retries", number_of_retries_); +} + +BT::NodeStatus RecoveryNode::tick() +{ + const unsigned children_count = children_nodes_.size(); + + if (children_count != 2) { + throw BT::BehaviorTreeException("Recovery Node '" + name() + "' must only have 2 children."); + } + + setStatus(BT::NodeStatus::RUNNING); + + while (current_child_idx_ < children_count && retry_count_ <= number_of_retries_) { + TreeNode * child_node = children_nodes_[current_child_idx_]; + const BT::NodeStatus child_status = child_node->executeTick(); + + if (current_child_idx_ == 0) { + switch (child_status) { + case BT::NodeStatus::SUCCESS: + { + // reset node and return success when first child returns success + halt(); + return BT::NodeStatus::SUCCESS; + } + + case BT::NodeStatus::FAILURE: + { + if (retry_count_ < number_of_retries_) { + // halt first child and tick second child in next iteration + ControlNode::haltChild(0); + current_child_idx_++; + break; + } else { + // reset node and return failure when max retries has been exceeded + halt(); + return BT::NodeStatus::FAILURE; + } + } + + case BT::NodeStatus::RUNNING: + { + return BT::NodeStatus::RUNNING; + } + + default: + { + throw BT::LogicError("A child node must never return IDLE"); + } + } // end switch + + } else if (current_child_idx_ == 1) { + switch (child_status) { + case BT::NodeStatus::SUCCESS: + { + // halt second child, increment recovery count, and tick first child in next iteration + ControlNode::haltChild(1); + retry_count_++; + current_child_idx_--; + } + break; + + case BT::NodeStatus::FAILURE: + { + // reset node and return failure if second child fails + halt(); + return BT::NodeStatus::FAILURE; + } + + case BT::NodeStatus::RUNNING: + { + return BT::NodeStatus::RUNNING; + } + + default: + { + throw BT::LogicError("A child node must never return IDLE"); + } + } // end switch + } + } // end while loop + + // reset node and return failure + halt(); + return BT::NodeStatus::FAILURE; +} + +void RecoveryNode::halt() +{ + ControlNode::halt(); + retry_count_ = 0; + current_child_idx_ = 0; +} + +} // namespace behaviortree_ros + +#include "behaviortree_cpp_v3/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("RecoveryNode"); +}