From 3d07a4a7acbffd9180104b3737402086059b022d Mon Sep 17 00:00:00 2001 From: Rebecca Williams Date: Thu, 25 Jan 2024 15:02:49 +0000 Subject: [PATCH 01/18] add in publisher test and shared resources test --- .../behaviortree_ros2/bt_topic_pub_node.hpp | 3 +- .../bt_topic_pub_shared_node.hpp | 200 ++++++++++++++++++ btcpp_ros2_samples/CMakeLists.txt | 10 + btcpp_ros2_samples/src/publisher_test.cpp | 69 ++++++ .../src/publisher_test_shared.cpp | 70 ++++++ 5 files changed, 350 insertions(+), 2 deletions(-) create mode 100644 behaviortree_ros2/include/behaviortree_ros2/bt_topic_pub_shared_node.hpp create mode 100644 btcpp_ros2_samples/src/publisher_test.cpp create mode 100644 btcpp_ros2_samples/src/publisher_test_shared.cpp diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_pub_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_pub_node.hpp index ebeb1f5..e0d1877 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_pub_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_pub_node.hpp @@ -92,11 +92,10 @@ class RosTopicPubNode : public BT::ConditionNode std::shared_ptr node_; std::string prev_topic_name_; bool topic_name_may_change_ = false; + std::shared_ptr publisher_; private: - std::shared_ptr publisher_; - bool createPublisher(const std::string& topic_name); }; diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_pub_shared_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_pub_shared_node.hpp new file mode 100644 index 0000000..895829f --- /dev/null +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_pub_shared_node.hpp @@ -0,0 +1,200 @@ +// Copyright (c) 2023 Davide Faconti, Unmanned Life +// +// 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 +#include +#include +#include "behaviortree_cpp/condition_node.h" +#include "behaviortree_cpp/bt_factory.h" +#include "behaviortree_ros2/ros_node_params.hpp" + +namespace BT +{ + +/** + * @brief Abstract class to wrap a ROS publisher + * + */ +template +class RosTopicPubSharedNode : public BT::ConditionNode +{ + +public: + // Type definitions + using Publisher = typename rclcpp::Publisher; + + /** 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 RosTopicPubSharedNode(const std::string & instance_name, + const BT::NodeConfig& conf, + const RosNodeParams& params); + + virtual ~RosTopicPubSharedNode() = default; + + /** + * @brief Any subclass of RosTopicPubSharedNode that has additinal ports 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("topic_name", "__default__placeholder__", "Topic name") + }; + 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({}); + } + + NodeStatus tick() override final; + + /** + * @brief setMessage is a callback invoked in tick to allow the user to pass + * the message to be published. + * + * @param msg the message. + * @return return false if anything is wrong and we must not send the message. + * the Condition will return FAILURE. + */ + virtual bool setMessage(TopicT& msg) = 0; + +protected: + + std::shared_ptr node_; + std::string prev_topic_name_; + bool topic_name_may_change_ = false; + bool initialized = false; + static bool shared_resource_initialized; + static std::shared_ptr publisher_; +private: + bool createPublisher(const std::string& topic_name); +}; + +//---------------------------------------------------------------- +//---------------------- DEFINITIONS ----------------------------- +//---------------------------------------------------------------- + +template inline + RosTopicPubSharedNode::RosTopicPubSharedNode(const std::string & instance_name, + const NodeConfig &conf, + const RosNodeParams& params) + : BT::ConditionNode(instance_name, conf), + node_(params.nh) +{ + // check port remapping + auto portIt = config().input_ports.find("topic_name"); + if(portIt != config().input_ports.end()) + { + const std::string& bb_topic_name = portIt->second; + + if(bb_topic_name.empty() || bb_topic_name == "__default__placeholder__") + { + if(params.default_port_value.empty()) { + throw std::logic_error( + "Both [topic_name] in the InputPort and the RosNodeParams are empty."); + } + else { + createPublisher(params.default_port_value); + } + } + else if(!isBlackboardPointer(bb_topic_name)) + { + // If the content of the port "topic_name" is not + // a pointer to the blackboard, but a static string, we can + // create the client in the constructor. + createPublisher(bb_topic_name); + } + else { + topic_name_may_change_ = true; + // createPublisher will be invoked in the first tick(). + } + } + else { + if(params.default_port_value.empty()) { + throw std::logic_error( + "Both [topic_name] in the InputPort and the RosNodeParams are empty."); + } + else { + createPublisher(params.default_port_value); + } + } +} + +template inline + bool RosTopicPubSharedNode::createPublisher(const std::string& topic_name) +{ + if(topic_name.empty()) + { + throw RuntimeError("topic_name is empty"); + } + + if (!shared_resource_initialized) { + publisher_ = node_->create_publisher(topic_name, 1); + prev_topic_name_ = topic_name; + shared_resource_initialized = true; + } + + return true; +} + +template inline + NodeStatus RosTopicPubSharedNode::tick() +{ + // First, check if the subscriber_ is valid and that the name of the + // topic_name in the port didn't change. + // otherwise, create a new subscriber + if(!publisher_ || (status() == NodeStatus::IDLE && topic_name_may_change_)) + { + std::string topic_name; + getInput("topic_name", topic_name); + if(prev_topic_name_ != topic_name) + { + createPublisher(topic_name); + } + } + + T msg; + if (!setMessage(msg)) + { + return NodeStatus::FAILURE; + } + publisher_->publish(msg); + return NodeStatus::SUCCESS; +} + +template +bool RosTopicPubSharedNode::shared_resource_initialized = false; + +template +typename std::shared_ptr> RosTopicPubSharedNode::publisher_ = nullptr; +} // namespace BT + diff --git a/btcpp_ros2_samples/CMakeLists.txt b/btcpp_ros2_samples/CMakeLists.txt index f2bb870..b85de5d 100644 --- a/btcpp_ros2_samples/CMakeLists.txt +++ b/btcpp_ros2_samples/CMakeLists.txt @@ -45,6 +45,14 @@ ament_target_dependencies(sleep_server ${THIS_PACKAGE_DEPS}) add_executable(subscriber_test src/subscriber_test.cpp) ament_target_dependencies(subscriber_test ${THIS_PACKAGE_DEPS}) +###################################################### +# Build publisher_tests +add_executable(publisher_test src/publisher_test.cpp) +ament_target_dependencies(publisher_test ${THIS_PACKAGE_DEPS}) + +add_executable(publisher_shared_test src/publisher_test_shared.cpp) +ament_target_dependencies(publisher_shared_test ${THIS_PACKAGE_DEPS}) + ###################################################### # INSTALL @@ -54,6 +62,8 @@ install(TARGETS sleep_server sleep_plugin subscriber_test + publisher_test + publisher_shared_test DESTINATION lib/${PROJECT_NAME} ) diff --git a/btcpp_ros2_samples/src/publisher_test.cpp b/btcpp_ros2_samples/src/publisher_test.cpp new file mode 100644 index 0000000..89a4b10 --- /dev/null +++ b/btcpp_ros2_samples/src/publisher_test.cpp @@ -0,0 +1,69 @@ +#include "behaviortree_ros2/bt_topic_pub_node.hpp" +#include +#include +#include "rclcpp/rclcpp.hpp" + +using namespace BT; + +class SendString: public RosTopicPubNode +{ +public: + SendString(const std::string& name, + const NodeConfig& conf, + const RosNodeParams& params) + : RosTopicPubNode(name, conf, params) + {} + + static BT::PortsList providedPorts() + { + BT::PortsList addition; + BT::PortsList basic = { + InputPort("message", "Hello World!", "Message to send") + }; + basic.insert(addition.begin(), addition.end()); + + return providedBasicPorts(basic); + } + + bool setMessage(std_msgs::msg::String& msg) + { + // RCLCPP_INFO(rclcpp::get_logger("test"), "publisher pointer: %p", reinterpret_cast(publisher_.get())); + std::string user_message = ""; + getInput("message", user_message); + msg.data = user_message; + return true; + } +}; + + // 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("publisher_test"); + + BehaviorTreeFactory factory; + + RosNodeParams params; + params.nh = nh; + params.default_port_value = "btcpp_string"; + factory.registerNodeType("SendString", params); + + auto tree = factory.createTreeFromText(xml_text); + + while(rclcpp::ok()) + { + tree.tickWhileRunning(); + } + return 0; +} diff --git a/btcpp_ros2_samples/src/publisher_test_shared.cpp b/btcpp_ros2_samples/src/publisher_test_shared.cpp new file mode 100644 index 0000000..99870f6 --- /dev/null +++ b/btcpp_ros2_samples/src/publisher_test_shared.cpp @@ -0,0 +1,70 @@ +#include "behaviortree_ros2/bt_topic_pub_shared_node.hpp" +#include +#include + +using namespace BT; + +class SendString: public RosTopicPubSharedNode +{ +public: + SendString(const std::string& name, + const NodeConfig& conf, + const RosNodeParams& params) + : RosTopicPubSharedNode(name, conf, params) + {} + + static BT::PortsList providedPorts() + { + BT::PortsList addition; + BT::PortsList basic = { + InputPort("message", "Hello World!", "Message to send") + }; + basic.insert(addition.begin(), addition.end()); + + return providedBasicPorts(basic); + } + + bool setMessage(std_msgs::msg::String& msg) + { + // RCLCPP_INFO(rclcpp::get_logger("test"), "publisher pointer: %p", reinterpret_cast(publisher_.get())); + std::string user_message = ""; + getInput("message", user_message); + msg.data = user_message; + return true; + } +}; + + // 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("publisher_shared_test"); + + BehaviorTreeFactory factory; + + RosNodeParams params; + params.nh = nh; + params.default_port_value = "btcpp_string"; + factory.registerNodeType("SendString", params); + factory.registerNodeType("SendInt", params); + + auto tree = factory.createTreeFromText(xml_text); + + while(rclcpp::ok()) + { + tree.tickWhileRunning(); + } + return 0; +} From ec6313c73c65656dafcbe93b4c44c7e5b3e43360 Mon Sep 17 00:00:00 2001 From: Rebecca Williams Date: Thu, 25 Jan 2024 15:47:42 +0000 Subject: [PATCH 02/18] remove unused variable --- .../include/behaviortree_ros2/bt_topic_pub_shared_node.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_pub_shared_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_pub_shared_node.hpp index 895829f..29e9c99 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_pub_shared_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_pub_shared_node.hpp @@ -92,7 +92,6 @@ class RosTopicPubSharedNode : public BT::ConditionNode std::shared_ptr node_; std::string prev_topic_name_; bool topic_name_may_change_ = false; - bool initialized = false; static bool shared_resource_initialized; static std::shared_ptr publisher_; private: From 2f5726c0b320c450c1098e6cd3a4c790ea64382c Mon Sep 17 00:00:00 2001 From: Rebecca Williams Date: Thu, 25 Jan 2024 15:48:10 +0000 Subject: [PATCH 03/18] make sub shared resources --- .../bt_topic_sub_shared_node.hpp | 312 ++++++++++++++++++ btcpp_ros2_samples/CMakeLists.txt | 12 +- .../src/publisher_test_shared.cpp | 1 - .../src/subscriber_shared_test.cpp | 64 ++++ btcpp_ros2_samples/src/subscriber_test.cpp | 1 + 5 files changed, 385 insertions(+), 5 deletions(-) create mode 100644 behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_shared_node.hpp create mode 100644 btcpp_ros2_samples/src/subscriber_shared_test.cpp diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_shared_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_shared_node.hpp new file mode 100644 index 0000000..2888b82 --- /dev/null +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_shared_node.hpp @@ -0,0 +1,312 @@ +// Copyright (c) 2023 Davide Faconti, Unmanned Life +// +// 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 +#include +#include +#include "behaviortree_cpp/condition_node.h" +#include "behaviortree_cpp/bt_factory.h" + +#include "behaviortree_ros2/ros_node_params.hpp" +#include + +namespace BT +{ + + +/** + * @brief Abstract class to wrap a Topic subscriber. + * Considering the example in the tutorial: + * https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Writing-A-Simple-Cpp-Publisher-And-Subscriber.html + * + * The corresponding wrapper would be: + * + * class SubscriberNode: RosTopicSubSharedNode + * + * The name of the topic will be determined as follows: + * + * 1. If a value is passes in the InputPort "topic_name", use that + * 2. Otherwise, use the value in RosNodeParams::default_port_value + */ +template +class RosTopicSubSharedNode : public BT::ConditionNode +{ + public: + // Type definitions + using Subscriber = typename rclcpp::Subscription; + + protected: + struct SubscriberInstance + { + void init(std::shared_ptr node, const std::string& topic_name) + { + if (!shared_resource_initialized) { + // create a callback group for this particular instance + callback_group = + node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); + callback_group_executor.add_callback_group( + callback_group, node->get_node_base_interface()); + + rclcpp::SubscriptionOptions option; + option.callback_group = callback_group; + + // The callback will broadcast to all the instances of RosTopicSubSharedNode + auto callback = [this](const std::shared_ptr msg) + { + broadcaster(msg); + }; + subscriber = node->create_subscription(topic_name, 1, callback, option); + } + } + + std::shared_ptr subscriber; + rclcpp::CallbackGroup::SharedPtr callback_group; + rclcpp::executors::SingleThreadedExecutor callback_group_executor; + boost::signals2::signal)> broadcaster; + + + }; + + static std::mutex& registryMutex() + { + static std::mutex sub_mutex; + return sub_mutex; + } + + // contains the fully-qualified name of the node and the name of the topic + static std::unordered_map>& getRegistry() + { + static std::unordered_map> subscribers_registry; + return subscribers_registry; + } + + std::shared_ptr node_; + std::shared_ptr sub_instance_ = nullptr; + std::shared_ptr last_msg_; + std::string topic_name_; + boost::signals2::connection signal_connection_; + std::string subscriber_key_; + static bool shared_resource_initialized; + + rclcpp::Logger logger() + { + return node_->get_logger(); + } + + public: + + /** 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 RosTopicSubSharedNode(const std::string & instance_name, + const BT::NodeConfig& conf, + const RosNodeParams& params); + + virtual ~RosTopicSubSharedNode() + { + signal_connection_.disconnect(); + // remove the subscribers from the static registry when the ALL the + // instances of RosTopicSubSharedNode are destroyed (i.e., the tree is destroyed) + if(sub_instance_) + { + sub_instance_.reset(); + std::unique_lock lk(registryMutex()); + auto& registry = getRegistry(); + auto it = registry.find(subscriber_key_); + // when the reference count is 1, means that the only instance is owned by the + // registry itself. Delete it + if(it != registry.end() && it->second.use_count() <= 1) + { + registry.erase(it); + RCLCPP_INFO(logger(), "Remove subscriber [%s]", topic_name_.c_str() ); + } + } + } + + /** + * @brief Any subclass of RosTopicNode 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("topic_name", "__default__placeholder__", "Topic name") + }; + 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({}); + } + + NodeStatus tick() override final; + + /** Callback invoked in the tick. You must return either SUCCESS of FAILURE + * + * @param last_msg the latest message received, since the last tick. + * Will be empty if no new message received. + * + * @return the new status of the Node, based on last_msg + */ + virtual NodeStatus onTick(const std::shared_ptr& last_msg) = 0; + +private: + + bool createSubscriber(const std::string& topic_name); +}; + +//---------------------------------------------------------------- +//---------------------- DEFINITIONS ----------------------------- +//---------------------------------------------------------------- + +template inline + RosTopicSubSharedNode::RosTopicSubSharedNode(const std::string & instance_name, + const NodeConfig &conf, + const RosNodeParams& params) + : BT::ConditionNode(instance_name, conf), + node_(params.nh) +{ + // check port remapping + auto portIt = config().input_ports.find("topic_name"); + if(portIt != config().input_ports.end()) + { + const std::string& bb_topic_name = portIt->second; + + if(bb_topic_name.empty() || bb_topic_name == "__default__placeholder__") + { + if(params.default_port_value.empty()) { + throw std::logic_error( + "Both [topic_name] in the InputPort and the RosNodeParams are empty."); + } + else { + createSubscriber(params.default_port_value); + } + } + else if(!isBlackboardPointer(bb_topic_name)) + { + // If the content of the port "topic_name" is not + // a pointer to the blackboard, but a static string, we can + // create the client in the constructor. + createSubscriber(bb_topic_name); + } + else { + // do nothing + // createSubscriber will be invoked in the first tick(). + } + } + else { + if(params.default_port_value.empty()) { + throw std::logic_error( + "Both [topic_name] in the InputPort and the RosNodeParams are empty."); + } + else { + createSubscriber(params.default_port_value); + } + } +} + +template inline + bool RosTopicSubSharedNode::createSubscriber(const std::string& topic_name) +{ + if(topic_name.empty()) + { + throw RuntimeError("topic_name is empty"); + } + if(sub_instance_) + { + throw RuntimeError("Can't call createSubscriber more than once"); + } + + // find SubscriberInstance in the registry + std::unique_lock lk(registryMutex()); + subscriber_key_ = std::string(node_->get_fully_qualified_name()) + "/" + topic_name; + + auto& registry = getRegistry(); + auto it = registry.find(subscriber_key_); + if(it == registry.end()) + { + it = registry.insert( {subscriber_key_, std::make_shared() }).first; + sub_instance_ = it->second; + sub_instance_->init(node_, topic_name); + + RCLCPP_INFO(logger(), + "Node [%s] created Subscriber to topic [%s]", + name().c_str(), topic_name.c_str() ); + } + else { + sub_instance_ = it->second; + } + + + // add "this" as received of the broadcaster + signal_connection_ = sub_instance_->broadcaster.connect( + [this](const std::shared_ptr msg) { last_msg_ = msg; } ); + + topic_name_ = topic_name; + return true; +} + + +template inline + NodeStatus RosTopicSubSharedNode::tick() +{ + // First, check if the subscriber_ is valid and that the name of the + // topic_name in the port didn't change. + // otherwise, create a new subscriber + if(!sub_instance_) + { + std::string topic_name; + getInput("topic_name", topic_name); + createSubscriber(topic_name); + } + + auto CheckStatus = [](NodeStatus status) + { + if( !isStatusCompleted(status) ) + { + throw std::logic_error("RosTopicSubSharedNode: the callback must return" + "either SUCCESS or FAILURE"); + } + return status; + }; + sub_instance_->callback_group_executor.spin_some(); + auto status = CheckStatus (onTick(last_msg_)); + last_msg_ = nullptr; + + return status; +} + +template +bool RosTopicSubSharedNode::shared_resource_initialized = false; + +// template +// typename std::shared_ptr> RosTopicPubSharedNode::publisher_ = nullptr; + +} // namespace BT + diff --git a/btcpp_ros2_samples/CMakeLists.txt b/btcpp_ros2_samples/CMakeLists.txt index b85de5d..298aa12 100644 --- a/btcpp_ros2_samples/CMakeLists.txt +++ b/btcpp_ros2_samples/CMakeLists.txt @@ -10,7 +10,7 @@ find_package(behaviortree_ros2 REQUIRED) find_package(std_msgs REQUIRED) find_package(btcpp_ros2_interfaces REQUIRED) -set(THIS_PACKAGE_DEPS +set(THIS_PACKAGE_DEPS behaviortree_ros2 std_msgs btcpp_ros2_interfaces ) @@ -18,7 +18,7 @@ set(THIS_PACKAGE_DEPS ###################################################### # Build a client that call the sleep action (STATIC version) -add_executable(sleep_client +add_executable(sleep_client src/sleep_action.cpp src/sleep_client.cpp) ament_target_dependencies(sleep_client ${THIS_PACKAGE_DEPS}) @@ -38,12 +38,15 @@ ament_target_dependencies(sleep_client_dyn ${THIS_PACKAGE_DEPS}) ###################################################### # Build Server add_executable(sleep_server src/sleep_server.cpp) -ament_target_dependencies(sleep_server ${THIS_PACKAGE_DEPS}) +ament_target_dependencies(sleep_server ${THIS_PACKAGE_DEPS}) ###################################################### # Build subscriber_test add_executable(subscriber_test src/subscriber_test.cpp) -ament_target_dependencies(subscriber_test ${THIS_PACKAGE_DEPS}) +ament_target_dependencies(subscriber_test ${THIS_PACKAGE_DEPS}) + +add_executable(subscriber_shared_test src/subscriber_shared_test.cpp) +ament_target_dependencies(subscriber_shared_test ${THIS_PACKAGE_DEPS}) ###################################################### # Build publisher_tests @@ -62,6 +65,7 @@ install(TARGETS sleep_server sleep_plugin subscriber_test + subscriber_shared_test publisher_test publisher_shared_test DESTINATION lib/${PROJECT_NAME} diff --git a/btcpp_ros2_samples/src/publisher_test_shared.cpp b/btcpp_ros2_samples/src/publisher_test_shared.cpp index 99870f6..7e98028 100644 --- a/btcpp_ros2_samples/src/publisher_test_shared.cpp +++ b/btcpp_ros2_samples/src/publisher_test_shared.cpp @@ -58,7 +58,6 @@ int main(int argc, char **argv) params.nh = nh; params.default_port_value = "btcpp_string"; factory.registerNodeType("SendString", params); - factory.registerNodeType("SendInt", params); auto tree = factory.createTreeFromText(xml_text); diff --git a/btcpp_ros2_samples/src/subscriber_shared_test.cpp b/btcpp_ros2_samples/src/subscriber_shared_test.cpp new file mode 100644 index 0000000..0fc0c34 --- /dev/null +++ b/btcpp_ros2_samples/src/subscriber_shared_test.cpp @@ -0,0 +1,64 @@ +#include "behaviortree_ros2/bt_topic_sub_shared_node.hpp" +#include + +using namespace BT; + +class ReceiveString: public RosTopicSubSharedNode +{ +public: + ReceiveString(const std::string& name, + const NodeConfig& conf, + const RosNodeParams& params) + : RosTopicSubSharedNode(name, conf, params) + {} + + static BT::PortsList providedPorts() + { + return {}; + } + + NodeStatus onTick(const std::shared_ptr& last_msg) override + { + RCLCPP_INFO(rclcpp::get_logger("test"), "subscriber pointer: %p", reinterpret_cast(sub_instance_->subscriber.get())); + if(last_msg) // empty if no new message received, since the last tick + { + RCLCPP_INFO(logger(), "[%s] new message: %s", name().c_str(), last_msg->data.c_str()); + } + return NodeStatus::SUCCESS; + } +}; + + // 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("subscriber_shared_test"); + + BehaviorTreeFactory factory; + + RosNodeParams params; + params.nh = nh; + params.default_port_value = "btcpp_string"; + factory.registerNodeType("ReceiveString", params); + + auto tree = factory.createTreeFromText(xml_text); + + while(rclcpp::ok()) + { + tree.tickWhileRunning(); + } + + return 0; +} diff --git a/btcpp_ros2_samples/src/subscriber_test.cpp b/btcpp_ros2_samples/src/subscriber_test.cpp index 195f1b9..132c596 100644 --- a/btcpp_ros2_samples/src/subscriber_test.cpp +++ b/btcpp_ros2_samples/src/subscriber_test.cpp @@ -19,6 +19,7 @@ class ReceiveString: public RosTopicSubNode NodeStatus onTick(const std::shared_ptr& last_msg) override { + RCLCPP_INFO(rclcpp::get_logger("test"), "subscriber pointer: %p", reinterpret_cast(sub_instance_->subscriber.get())); if(last_msg) // empty if no new message received, since the last tick { RCLCPP_INFO(logger(), "[%s] new message: %s", name().c_str(), last_msg->data.c_str()); From 866a232bfc4ff742edbf401fcf2da9246c326369 Mon Sep 17 00:00:00 2001 From: Rebecca Williams Date: Fri, 26 Jan 2024 12:07:43 +0000 Subject: [PATCH 04/18] WIP action node --- .../bt_action_error_code.hpp | 31 ++ .../behaviortree_ros2/bt_action_node.hpp | 29 +- .../bt_action_shared_node.hpp | 442 ++++++++++++++++++ btcpp_ros2_samples/CMakeLists.txt | 7 + btcpp_ros2_samples/src/sleep_action.cpp | 4 +- .../src/sleep_action_shared.cpp | 36 ++ .../src/sleep_action_shared.hpp | 27 ++ btcpp_ros2_samples/src/sleep_client.cpp | 3 +- .../src/sleep_client_shared.cpp | 90 ++++ 9 files changed, 640 insertions(+), 29 deletions(-) create mode 100644 behaviortree_ros2/include/behaviortree_ros2/bt_action_error_code.hpp create mode 100644 behaviortree_ros2/include/behaviortree_ros2/bt_action_shared_node.hpp create mode 100644 btcpp_ros2_samples/src/sleep_action_shared.cpp create mode 100644 btcpp_ros2_samples/src/sleep_action_shared.hpp create mode 100644 btcpp_ros2_samples/src/sleep_client_shared.cpp diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_action_error_code.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_action_error_code.hpp new file mode 100644 index 0000000..0d4ca74 --- /dev/null +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_action_error_code.hpp @@ -0,0 +1,31 @@ +#ifndef BEHAVIORTREE_ROS2__BT_ACTION_ERROR_CODE_HPP_ +#define BEHAVIORTREE_ROS2__BT_ACTION_ERROR_CODE_HPP_ + +namespace BT +{ +enum ActionNodeErrorCode +{ + SERVER_UNREACHABLE, + SEND_GOAL_TIMEOUT, + GOAL_REJECTED_BY_SERVER, + ACTION_ABORTED, + ACTION_CANCELLED, + INVALID_GOAL +}; + +inline const char* toStr(const ActionNodeErrorCode& err) +{ + switch (err) + { + case SERVER_UNREACHABLE: return "SERVER_UNREACHABLE"; + case SEND_GOAL_TIMEOUT: return "SEND_GOAL_TIMEOUT"; + case GOAL_REJECTED_BY_SERVER: return "GOAL_REJECTED_BY_SERVER"; + case ACTION_ABORTED: return "ACTION_ABORTED"; + case ACTION_CANCELLED: return "ACTION_CANCELLED"; + case INVALID_GOAL: return "INVALID_GOAL"; + } + return nullptr; +} +} // namespace BT + +#endif // BEHAVIORTREE_ROS2__BT_ACTION_ERROR_CODE_HPP_ diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp index 8e4d49f..a67e7cf 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp @@ -24,34 +24,10 @@ #include "rclcpp_action/rclcpp_action.hpp" #include "behaviortree_ros2/ros_node_params.hpp" +#include "behaviortree_ros2/bt_action_error_code.hpp" namespace BT { - -enum ActionNodeErrorCode -{ - SERVER_UNREACHABLE, - SEND_GOAL_TIMEOUT, - GOAL_REJECTED_BY_SERVER, - ACTION_ABORTED, - ACTION_CANCELLED, - INVALID_GOAL -}; - -inline const char* toStr(const ActionNodeErrorCode& err) -{ - switch (err) - { - case SERVER_UNREACHABLE: return "SERVER_UNREACHABLE"; - case SEND_GOAL_TIMEOUT: return "SEND_GOAL_TIMEOUT"; - case GOAL_REJECTED_BY_SERVER: return "GOAL_REJECTED_BY_SERVER"; - case ACTION_ABORTED: return "ACTION_ABORTED"; - case ACTION_CANCELLED: return "ACTION_CANCELLED"; - case INVALID_GOAL: return "INVALID_GOAL"; - } - return nullptr; -} - /** * @brief Abstract class to wrap rclcpp_action::Client<> * @@ -172,10 +148,9 @@ class RosActionNode : public BT::ActionNodeBase bool action_name_may_change_ = false; const std::chrono::milliseconds server_timeout_; const std::chrono::milliseconds wait_for_server_timeout_; - + ActionClientPtr action_client_; private: - ActionClientPtr action_client_; rclcpp::CallbackGroup::SharedPtr callback_group_; rclcpp::executors::SingleThreadedExecutor callback_group_executor_; diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_action_shared_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_action_shared_node.hpp new file mode 100644 index 0000000..ac2879a --- /dev/null +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_action_shared_node.hpp @@ -0,0 +1,442 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2023 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. + +#pragma once + +#include +#include +#include +#include +#include "behaviortree_cpp/action_node.h" +#include "behaviortree_cpp/bt_factory.h" +#include "rclcpp_action/rclcpp_action.hpp" +#include "behaviortree_ros2/bt_action_error_code.hpp" + +#include "behaviortree_ros2/ros_node_params.hpp" + +namespace BT +{ +/** + * @brief Abstract class to wrap rclcpp_action::Client<> + * + * For instance, given the type AddTwoInts described in this tutorial: + * https://docs.ros.org/en/humble/Tutorials/Intermediate/Writing-an-Action-Server-Client/Cpp.html + * + * the corresponding wrapper would be: + * + * class FibonacciNode: public RosActionSharedNode + * + * RosActionSharedNode will try to be non-blocking for the entire duration of the call. + * The derived class must reimplement the virtual methods as described below. + * + * The name of the action will be determined as follows: + * + * 1. If a value is passes in the InputPort "action_name", use that + * 2. Otherwise, use the value in RosNodeParams::default_port_value + */ +template +class RosActionSharedNode : public BT::ActionNodeBase +{ + +public: + // Type definitions + using ActionType = ActionT; + using ActionClient = typename rclcpp_action::Client; + using ActionClientPtr = std::shared_ptr; + using Goal = typename ActionT::Goal; + using GoalHandle = typename rclcpp_action::ClientGoalHandle; + using WrappedResult = typename rclcpp_action::ClientGoalHandle::WrappedResult; + using Feedback = typename ActionT::Feedback; + + /** To register this class into the factory, use: + * + * factory.registerNodeType<>(node_name, params); + * + */ + explicit RosActionSharedNode(const std::string & instance_name, + const BT::NodeConfig& conf, + const RosNodeParams& params); + + virtual ~RosActionSharedNode() = default; + + /** + * @brief Any subclass of RosActionSharedNode that has ports must implement 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("action_name", "__default__placeholder__", "Action server name") + }; + 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 Callback executed when the node is halted. Note that cancelGoal() + /// is done automatically. + virtual void onHalt() {} + + /** setGoal s a callback that allows the user to set + * the goal message (ActionT::Goal). + * + * @param goal the goal to be sent to the action server. + * + * @return false if the request should not be sent. In that case, + * RosActionSharedNode::onFailure(INVALID_GOAL) will be called. + */ + virtual bool setGoal(Goal& goal) = 0; + + /** 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; + + /** 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 onFeedback(const std::shared_ptr /*feedback*/) + { + return NodeStatus::RUNNING; + } + + /** Callback invoked when something goes wrong. + * It must return either SUCCESS or FAILURE. + */ + virtual BT::NodeStatus onFailure(ActionNodeErrorCode /*error*/) + { + return NodeStatus::FAILURE; + } + + /// Method used to send a request to the Action server to cancel the current goal + void cancelGoal(); + + + /// The default halt() implementation will call cancelGoal if necessary. + void halt() override final; + + NodeStatus tick() override final; + +protected: + + std::shared_ptr node_; + std::string prev_action_name_; + bool action_name_may_change_ = false; + const std::chrono::milliseconds server_timeout_; + const std::chrono::milliseconds wait_for_server_timeout_; + static bool shared_resource_initialized; + static ActionClientPtr action_client_; + +private: + + rclcpp::CallbackGroup::SharedPtr callback_group_; + rclcpp::executors::SingleThreadedExecutor callback_group_executor_; + + std::shared_future future_goal_handle_; + typename GoalHandle::SharedPtr goal_handle_; + + rclcpp::Time time_goal_sent_; + NodeStatus on_feedback_state_change_; + bool goal_received_; + WrappedResult result_; + + bool createClient(const std::string &action_name); +}; + +//---------------------------------------------------------------- +//---------------------- DEFINITIONS ----------------------------- +//---------------------------------------------------------------- + +template inline + RosActionSharedNode::RosActionSharedNode(const std::string & instance_name, + const NodeConfig &conf, + const RosNodeParams ¶ms): + BT::ActionNodeBase(instance_name, conf), + node_(params.nh), + server_timeout_(params.server_timeout), + wait_for_server_timeout_(params.wait_for_server_timeout) +{ + // Three cases: + // - we use the default action_name in RosNodeParams when port is empty + // - we use the action_name in the port and it is a static string. + // - we use the action_name in the port and it is blackboard entry. + + // check port remapping + auto portIt = config().input_ports.find("action_name"); + if(portIt != config().input_ports.end()) + { + const std::string& bb_action_name = portIt->second; + + if(bb_action_name.empty() || bb_action_name == "__default__placeholder__") + { + if(params.default_port_value.empty()) { + throw std::logic_error( + "Both [action_name] in the InputPort and the RosNodeParams are empty."); + } + else { + createClient(params.default_port_value); + } + } + else if(!isBlackboardPointer(bb_action_name)) + { + // If the content of the port "action_name" is not + // a pointer to the blackboard, but a static string, we can + // create the client in the constructor. + createClient(bb_action_name); + } + else { + action_name_may_change_ = true; + // createClient will be invoked in the first tick(). + } + } + else { + + if(params.default_port_value.empty()) { + throw std::logic_error( + "Both [action_name] in the InputPort and the RosNodeParams are empty."); + } + else { + createClient(params.default_port_value); + } + } +} + +template inline + bool RosActionSharedNode::createClient(const std::string& action_name) +{ + if(action_name.empty()) + { + throw RuntimeError("action_name is empty"); + } + + if (!shared_resource_initialized) { + callback_group_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); + action_client_ = rclcpp_action::create_client(node_, action_name, callback_group_); + + prev_action_name_ = action_name; + shared_resource_initialized = true; + } + + bool found = action_client_->wait_for_action_server(wait_for_server_timeout_); + if(!found) + { + RCLCPP_ERROR(node_->get_logger(), "%s: Action server with name '%s' is not reachable.", + name().c_str(), prev_action_name_.c_str()); + } + return found; +} + +template inline + NodeStatus RosActionSharedNode::tick() +{ + // First, check if the action_client_ is valid and that the name of the + // action_name in the port didn't change. + // otherwise, create a new client + if(!action_client_ || (status() == NodeStatus::IDLE && action_name_may_change_)) + { + std::string action_name; + getInput("action_name", action_name); + if(prev_action_name_ != action_name) + { + createClient(action_name); + } + } + + //------------------------------------------ + auto CheckStatus = [](NodeStatus status) + { + if( !isStatusCompleted(status) ) + { + throw std::logic_error("RosActionSharedNode: the callback must return either SUCCESS of FAILURE"); + } + return status; + }; + + // first step to be done only at the beginning of the Action + if (status() == BT::NodeStatus::IDLE) + { + setStatus(NodeStatus::RUNNING); + + goal_received_ = false; + future_goal_handle_ = {}; + on_feedback_state_change_ = NodeStatus::RUNNING; + result_ = {}; + + Goal goal; + + if( !setGoal(goal) ) + { + return CheckStatus( onFailure(INVALID_GOAL) ); + } + + typename ActionClient::SendGoalOptions goal_options; + + //-------------------- + goal_options.feedback_callback = + [this](typename GoalHandle::SharedPtr, + const std::shared_ptr feedback) + { + on_feedback_state_change_ = onFeedback(feedback); + if( on_feedback_state_change_ == NodeStatus::IDLE) + { + throw std::logic_error("onFeedback must not return IDLE"); + } + emitWakeUpSignal(); + }; + //-------------------- + goal_options.result_callback = + [this](const WrappedResult& result) + { + if (goal_handle_->get_goal_id() == result.goal_id) { + RCLCPP_DEBUG( node_->get_logger(), "result_callback" ); + result_ = result; + emitWakeUpSignal(); + } + }; + //-------------------- + goal_options.goal_response_callback = + [this](typename GoalHandle::SharedPtr const future_handle) + { + auto goal_handle_ = future_handle.get(); + if (!goal_handle_) + { + RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server"); + } else { + RCLCPP_DEBUG(node_->get_logger(), "Goal accepted by server, waiting for result"); + } + }; + //-------------------- + + future_goal_handle_ = action_client_->async_send_goal( goal, goal_options ); + time_goal_sent_ = node_->now(); + + return NodeStatus::RUNNING; + } + + if (status() == NodeStatus::RUNNING) + { + RCLCPP_INFO(rclcpp::get_logger("test"), "check1"); + callback_group_executor_.spin_some(); + RCLCPP_INFO(rclcpp::get_logger("test"), "check6"); + + // FIRST case: check if the goal request has a timeout + if( !goal_received_ ) + { + RCLCPP_INFO(rclcpp::get_logger("test"), "check2"); + auto nodelay = std::chrono::milliseconds(0); + auto timeout = rclcpp::Duration::from_seconds( double(server_timeout_.count()) / 1000); + + auto ret = callback_group_executor_.spin_until_future_complete(future_goal_handle_, nodelay); + if (ret != rclcpp::FutureReturnCode::SUCCESS) + { + if( (node_->now() - time_goal_sent_) > timeout ) + { + return CheckStatus( onFailure(SEND_GOAL_TIMEOUT) ); + } + else{ + return NodeStatus::RUNNING; + } + } + else + { + RCLCPP_INFO(rclcpp::get_logger("test"), "check3"); + goal_received_ = true; + goal_handle_ = future_goal_handle_.get(); + future_goal_handle_ = {}; + + if (!goal_handle_) { + return CheckStatus( onFailure( GOAL_REJECTED_BY_SERVER ) ); + } + } + } + + // SECOND case: onFeedback requested a stop + if( on_feedback_state_change_ != NodeStatus::RUNNING ) + + { + RCLCPP_INFO(rclcpp::get_logger("test"), "check4"); + cancelGoal(); + return on_feedback_state_change_; + } + // THIRD case: result received, requested a stop + RCLCPP_INFO(rclcpp::get_logger("test"), "check5"); + 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_ ) ); + } + } + } + return NodeStatus::RUNNING; +} + +template inline + void RosActionSharedNode::halt() +{ + if(status() == BT::NodeStatus::RUNNING) + { + cancelGoal(); + onHalt(); + } +} + +template inline + void RosActionSharedNode::cancelGoal() +{ + auto future_result = action_client_->async_get_result(goal_handle_); + 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]", + prev_action_name_.c_str()); + } + + if (callback_group_executor_.spin_until_future_complete(future_result, server_timeout_) != + rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR( node_->get_logger(), "Failed to get result call failed :( for [%s]", + prev_action_name_.c_str()); + } +} + +template +bool RosActionSharedNode::shared_resource_initialized = false; + +template +typename std::shared_ptr> RosActionSharedNode::action_client_ = nullptr; +} // namespace BT diff --git a/btcpp_ros2_samples/CMakeLists.txt b/btcpp_ros2_samples/CMakeLists.txt index 298aa12..7f7e651 100644 --- a/btcpp_ros2_samples/CMakeLists.txt +++ b/btcpp_ros2_samples/CMakeLists.txt @@ -23,6 +23,12 @@ add_executable(sleep_client src/sleep_client.cpp) ament_target_dependencies(sleep_client ${THIS_PACKAGE_DEPS}) + +add_executable(sleep_client_shared + src/sleep_action_shared.cpp + src/sleep_client_shared.cpp) +ament_target_dependencies(sleep_client_shared ${THIS_PACKAGE_DEPS}) + ###################################################### # Build a client that call the sleep action (Plugin version) @@ -61,6 +67,7 @@ ament_target_dependencies(publisher_shared_test ${THIS_PACKAGE_DEPS}) install(TARGETS sleep_client + sleep_client_shared sleep_client_dyn sleep_server sleep_plugin diff --git a/btcpp_ros2_samples/src/sleep_action.cpp b/btcpp_ros2_samples/src/sleep_action.cpp index a838d74..1754a54 100644 --- a/btcpp_ros2_samples/src/sleep_action.cpp +++ b/btcpp_ros2_samples/src/sleep_action.cpp @@ -10,8 +10,9 @@ bool SleepAction::setGoal(RosActionNode::Goal &goal) NodeStatus SleepAction::onResultReceived(const RosActionNode::WrappedResult &wr) { - RCLCPP_INFO( node_->get_logger(), "%s: onResultReceived. Done = %s", name().c_str(), + RCLCPP_INFO( node_->get_logger(), "%s: onResultReceived. Done = %s", name().c_str(), wr.result->done ? "true" : "false" ); + RCLCPP_INFO(rclcpp::get_logger("test"), " %s action pointer: %p", name().c_str(), reinterpret_cast(action_client_.get())); return wr.result->done ? NodeStatus::SUCCESS : NodeStatus::FAILURE; } @@ -25,6 +26,7 @@ NodeStatus SleepAction::onFailure(ActionNodeErrorCode error) void SleepAction::onHalt() { RCLCPP_INFO( node_->get_logger(), "%s: onHalt", name().c_str() ); + RCLCPP_INFO(rclcpp::get_logger("test"), " %s action pointer: %p", name().c_str(), reinterpret_cast(action_client_.get())); } // Plugin registration. diff --git a/btcpp_ros2_samples/src/sleep_action_shared.cpp b/btcpp_ros2_samples/src/sleep_action_shared.cpp new file mode 100644 index 0000000..5b271cd --- /dev/null +++ b/btcpp_ros2_samples/src/sleep_action_shared.cpp @@ -0,0 +1,36 @@ +#include "sleep_action_shared.hpp" +#include "behaviortree_ros2/plugins.hpp" + +bool SleepAction::setGoal(RosActionSharedNode::Goal &goal) +{ + RCLCPP_INFO(rclcpp::get_logger("test"), "%s, set goal", name().c_str()); + auto timeout = getInput("msec"); + goal.msec_timeout = timeout.value(); + return true; +} + +NodeStatus SleepAction::onResultReceived(const RosActionSharedNode::WrappedResult &wr) +{ + RCLCPP_INFO( node_->get_logger(), "%s: onResultReceived. Done = %s", name().c_str(), + wr.result->done ? "true" : "false" ); + RCLCPP_INFO(rclcpp::get_logger("test"), " %s action pointer: %p", name().c_str(), reinterpret_cast(action_client_.get())); + + return wr.result->done ? NodeStatus::SUCCESS : NodeStatus::FAILURE; +} + +NodeStatus SleepAction::onFailure(ActionNodeErrorCode error) +{ + RCLCPP_ERROR( node_->get_logger(), "%s: onFailure with error: %s", name().c_str(), toStr(error) ); + RCLCPP_INFO(rclcpp::get_logger("test"), " %s action pointer: %p", name().c_str(), reinterpret_cast(action_client_.get())); + return NodeStatus::FAILURE; +} + +void SleepAction::onHalt() +{ + RCLCPP_INFO( node_->get_logger(), "%s: onHalt", name().c_str() ); + RCLCPP_INFO(rclcpp::get_logger("test"), " %s action pointer: %p", name().c_str(), reinterpret_cast(action_client_.get())); +} + +// Plugin registration. +// The class SleepAction will self register with name "Sleep". +CreateRosNodePlugin(SleepAction, "Sleep"); diff --git a/btcpp_ros2_samples/src/sleep_action_shared.hpp b/btcpp_ros2_samples/src/sleep_action_shared.hpp new file mode 100644 index 0000000..b8c3ccc --- /dev/null +++ b/btcpp_ros2_samples/src/sleep_action_shared.hpp @@ -0,0 +1,27 @@ +#include "behaviortree_ros2/bt_action_shared_node.hpp" +#include "btcpp_ros2_interfaces/action/sleep.hpp" + +using namespace BT; + +class SleepAction: public RosActionSharedNode +{ +public: + SleepAction(const std::string& name, + const NodeConfig& conf, + const RosNodeParams& params) + : RosActionSharedNode(name, conf, params) + {} + + static BT::PortsList providedPorts() + { + return providedBasicPorts({InputPort("msec")}); + } + + bool setGoal(Goal& goal) override; + + void onHalt() override; + + BT::NodeStatus onResultReceived(const WrappedResult& wr) override; + + virtual BT::NodeStatus onFailure(ActionNodeErrorCode error) override; +}; diff --git a/btcpp_ros2_samples/src/sleep_client.cpp b/btcpp_ros2_samples/src/sleep_client.cpp index 2290f7e..2acf6e9 100644 --- a/btcpp_ros2_samples/src/sleep_client.cpp +++ b/btcpp_ros2_samples/src/sleep_client.cpp @@ -22,6 +22,7 @@ class PrintValue : public BT::SyncActionNode BT::NodeStatus tick() override { std::string msg; + if( getInput("message", msg ) ){ std::cout << "PrintValue: " << msg << std::endl; return NodeStatus::SUCCESS; @@ -45,7 +46,7 @@ class PrintValue : public BT::SyncActionNode - + diff --git a/btcpp_ros2_samples/src/sleep_client_shared.cpp b/btcpp_ros2_samples/src/sleep_client_shared.cpp new file mode 100644 index 0000000..09f9bfe --- /dev/null +++ b/btcpp_ros2_samples/src/sleep_client_shared.cpp @@ -0,0 +1,90 @@ +#include "behaviortree_ros2/bt_action_shared_node.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/executors.hpp" + +#include "behaviortree_ros2/plugins.hpp" + +#ifndef USE_SLEEP_PLUGIN +#include "sleep_action_shared.hpp" +#endif + +using namespace BT; + +//------------------------------------------------------------- +// Simple Action to print a number +//------------------------------------------------------------- + +class PrintValue : public BT::SyncActionNode +{ +public: + PrintValue(const std::string& name, const BT::NodeConfig& 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") }; + } +}; + +//----------------------------------------------------- + + // 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"); + + RosNodeParams params; + params.nh = nh; + params.default_port_value = "sleep_service"; + +#ifdef USE_SLEEP_PLUGIN + RegisterRosNode(factory, "../lib/libsleep_action_plugin.so", params); +#else + factory.registerNodeType("SleepAction", params); +#endif + + auto tree = factory.createTreeFromText(xml_text); + + for(int i=0; i<5; i++){ + tree.tickWhileRunning(); + } + + return 0; +} From 8a48aa6f09019b5b71f4fefadd119da6ce9ac49e Mon Sep 17 00:00:00 2001 From: Rebecca Williams Date: Fri, 26 Jan 2024 16:12:38 +0000 Subject: [PATCH 05/18] add in service test --- .../behaviortree_ros2/bt_service_node.hpp | 6 +- btcpp_ros2_interfaces/CMakeLists.txt | 3 +- btcpp_ros2_interfaces/srv/AddTwoInts.srv | 4 + btcpp_ros2_samples/CMakeLists.txt | 10 +++ .../src/add_two_ints_server.cpp | 28 ++++++ btcpp_ros2_samples/src/service_test.cpp | 85 +++++++++++++++++++ 6 files changed, 132 insertions(+), 4 deletions(-) create mode 100644 btcpp_ros2_interfaces/srv/AddTwoInts.srv create mode 100644 btcpp_ros2_samples/src/add_two_ints_server.cpp create mode 100644 btcpp_ros2_samples/src/service_test.cpp diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp index 66477e3..ea63990 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp @@ -133,7 +133,7 @@ class RosServiceNode : public BT::ActionNodeBase * It must return either SUCCESS or FAILURE. */ virtual BT::NodeStatus onFailure(ServiceNodeErrorCode /*error*/) - { + { return NodeStatus::FAILURE; } @@ -144,10 +144,10 @@ class RosServiceNode : public BT::ActionNodeBase bool service_name_may_change_ = false; const std::chrono::milliseconds service_timeout_; const std::chrono::milliseconds wait_for_service_timeout_; - + typename std::shared_ptr service_client_; private: - typename std::shared_ptr service_client_; + rclcpp::CallbackGroup::SharedPtr callback_group_; rclcpp::executors::SingleThreadedExecutor callback_group_executor_; diff --git a/btcpp_ros2_interfaces/CMakeLists.txt b/btcpp_ros2_interfaces/CMakeLists.txt index 49080ee..343b048 100644 --- a/btcpp_ros2_interfaces/CMakeLists.txt +++ b/btcpp_ros2_interfaces/CMakeLists.txt @@ -8,7 +8,8 @@ find_package(ament_cmake REQUIRED) find_package(rosidl_default_generators REQUIRED) rosidl_generate_interfaces(btcpp_ros2_interfaces - "action/Sleep.action") + "action/Sleep.action" + "srv/AddTwoInts.srv") ament_export_dependencies(rosidl_default_runtime) ament_package() diff --git a/btcpp_ros2_interfaces/srv/AddTwoInts.srv b/btcpp_ros2_interfaces/srv/AddTwoInts.srv new file mode 100644 index 0000000..3a68808 --- /dev/null +++ b/btcpp_ros2_interfaces/srv/AddTwoInts.srv @@ -0,0 +1,4 @@ +int64 a +int64 b +--- +int64 sum diff --git a/btcpp_ros2_samples/CMakeLists.txt b/btcpp_ros2_samples/CMakeLists.txt index 7f7e651..645781a 100644 --- a/btcpp_ros2_samples/CMakeLists.txt +++ b/btcpp_ros2_samples/CMakeLists.txt @@ -62,6 +62,14 @@ ament_target_dependencies(publisher_test ${THIS_PACKAGE_DEPS}) add_executable(publisher_shared_test src/publisher_test_shared.cpp) ament_target_dependencies(publisher_shared_test ${THIS_PACKAGE_DEPS}) +###################################################### +# Build service_tests +add_executable(add_two_ints_server src/add_two_ints_server.cpp) +ament_target_dependencies(add_two_ints_server ${THIS_PACKAGE_DEPS}) + +add_executable(service_test src/service_test.cpp) +ament_target_dependencies(service_test ${THIS_PACKAGE_DEPS}) + ###################################################### # INSTALL @@ -75,6 +83,8 @@ install(TARGETS subscriber_shared_test publisher_test publisher_shared_test + add_two_ints_server + service_test DESTINATION lib/${PROJECT_NAME} ) diff --git a/btcpp_ros2_samples/src/add_two_ints_server.cpp b/btcpp_ros2_samples/src/add_two_ints_server.cpp new file mode 100644 index 0000000..90cceee --- /dev/null +++ b/btcpp_ros2_samples/src/add_two_ints_server.cpp @@ -0,0 +1,28 @@ +#include "rclcpp/rclcpp.hpp" +#include "btcpp_ros2_interfaces/srv/add_two_ints.hpp" + +#include + +void add(const std::shared_ptr request, + std::shared_ptr response) +{ + response->sum = request->a + request->b; + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Incoming request\na: %ld" " b: %ld", + request->a, request->b); + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "sending back response: [%ld]", (long int)response->sum); +} + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + + std::shared_ptr node = rclcpp::Node::make_shared("add_two_ints_server"); + + rclcpp::Service::SharedPtr service = + node->create_service("add_two_ints", &add); + + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Ready to add two ints."); + + rclcpp::spin(node); + rclcpp::shutdown(); +} diff --git a/btcpp_ros2_samples/src/service_test.cpp b/btcpp_ros2_samples/src/service_test.cpp new file mode 100644 index 0000000..4a574f1 --- /dev/null +++ b/btcpp_ros2_samples/src/service_test.cpp @@ -0,0 +1,85 @@ +#include "behaviortree_ros2/bt_service_node.hpp" +#include "btcpp_ros2_interfaces/srv/add_two_ints.hpp" + +using namespace BT; + +class AddTwoIntsClient: public RosServiceNode +{ +public: + AddTwoIntsClient(const std::string& name, + const NodeConfig& conf, + const RosNodeParams& params) + : RosServiceNode(name, conf, params) + {} + + static PortsList providedPorts() + { + BT::PortsList addition; + BT::PortsList basic = { + InputPort("a", 12, "first integer to add"), + InputPort("b", 12, "second integer to add") + }; + basic.insert(addition.begin(), addition.end()); + return providedBasicPorts(basic); + } + + bool setRequest(Request::SharedPtr& req) override + { + getInput("a", a); + getInput("b", b); + req->a = a; + req->b = b; + + return true; + } + + virtual BT::NodeStatus onResponseReceived(const Response::SharedPtr& res) override + { + RCLCPP_INFO(rclcpp::get_logger("test"), "pointer: %p", reinterpret_cast(service_client_.get())); + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Result of %d + %d = %ld", + a, b, res->sum); + return BT::NodeStatus::SUCCESS; + } + + virtual BT::NodeStatus onFailure(ServiceNodeErrorCode error_code) override + { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "AddTwoIntsClient failed with error code %d", error_code); + return BT::NodeStatus::FAILURE; + } +private: + int a; + int b; +}; + + // 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("service_test"); + + BehaviorTreeFactory factory; + + RosNodeParams params; + params.nh = nh; + params.default_port_value = "btcpp_service"; + factory.registerNodeType("AddTwoIntsClient", params); + + auto tree = factory.createTreeFromText(xml_text); + + while(rclcpp::ok()) + { + tree.tickWhileRunning(); + } + return 0; +} From d7fd541efdfb44441dab91d7fe7e931600636e95 Mon Sep 17 00:00:00 2001 From: Rebecca Williams Date: Fri, 26 Jan 2024 16:55:16 +0000 Subject: [PATCH 06/18] action shared and service shared --- .../bt_action_shared_node.hpp | 59 +-- .../bt_service_error_code.hpp | 27 ++ .../behaviortree_ros2/bt_service_node.hpp | 22 +- .../bt_service_shared_node.hpp | 338 ++++++++++++++++++ btcpp_ros2_samples/CMakeLists.txt | 4 + .../src/service_shared_test.cpp | 85 +++++ 6 files changed, 494 insertions(+), 41 deletions(-) create mode 100644 behaviortree_ros2/include/behaviortree_ros2/bt_service_error_code.hpp create mode 100644 behaviortree_ros2/include/behaviortree_ros2/bt_service_shared_node.hpp create mode 100644 btcpp_ros2_samples/src/service_shared_test.cpp diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_action_shared_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_action_shared_node.hpp index ac2879a..e55f4e0 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_action_shared_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_action_shared_node.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include #include "behaviortree_cpp/action_node.h" @@ -150,11 +151,15 @@ class RosActionSharedNode : public BT::ActionNodeBase const std::chrono::milliseconds wait_for_server_timeout_; static bool shared_resource_initialized; static ActionClientPtr action_client_; + static std::mutex action_client_mutex_; + private: rclcpp::CallbackGroup::SharedPtr callback_group_; - rclcpp::executors::SingleThreadedExecutor callback_group_executor_; + static std::shared_ptr callback_group_executor_; + static std::mutex static_members_mutex_; + std::shared_future future_goal_handle_; typename GoalHandle::SharedPtr goal_handle_; @@ -226,16 +231,23 @@ template inline } template inline - bool RosActionSharedNode::createClient(const std::string& action_name) +bool RosActionSharedNode::createClient(const std::string& action_name) { - if(action_name.empty()) + std::lock_guard lock(static_members_mutex_); + if (action_name.empty()) { - throw RuntimeError("action_name is empty"); + throw RuntimeError("action_name is empty"); } - if (!shared_resource_initialized) { + if (!shared_resource_initialized) + { + RCLCPP_INFO(node_->get_logger(), "Creating action client for %s", action_name.c_str()); callback_group_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); + if (callback_group_executor_ == nullptr) + { + callback_group_executor_ = std::make_shared(); + } + callback_group_executor_->add_callback_group(callback_group_, node_->get_node_base_interface()); action_client_ = rclcpp_action::create_client(node_, action_name, callback_group_); prev_action_name_ = action_name; @@ -259,6 +271,7 @@ template inline // otherwise, create a new client if(!action_client_ || (status() == NodeStatus::IDLE && action_name_may_change_)) { + RCLCPP_INFO(node_->get_logger(), "Client not initialized or action_name changed"); std::string action_name; getInput("action_name", action_name); if(prev_action_name_ != action_name) @@ -280,6 +293,7 @@ template inline // first step to be done only at the beginning of the Action if (status() == BT::NodeStatus::IDLE) { + RCLCPP_INFO(node_->get_logger(), "Client IDLE sending goal"); setStatus(NodeStatus::RUNNING); goal_received_ = false; @@ -331,7 +345,7 @@ template inline } }; //-------------------- - + RCLCPP_INFO(node_->get_logger(), "Goal sent to server"); future_goal_handle_ = action_client_->async_send_goal( goal, goal_options ); time_goal_sent_ = node_->now(); @@ -340,18 +354,16 @@ template inline if (status() == NodeStatus::RUNNING) { - RCLCPP_INFO(rclcpp::get_logger("test"), "check1"); - callback_group_executor_.spin_some(); - RCLCPP_INFO(rclcpp::get_logger("test"), "check6"); - + std::lock_guard lock(action_client_mutex_); + callback_group_executor_->spin_some(); // FIRST case: check if the goal request has a timeout if( !goal_received_ ) { - RCLCPP_INFO(rclcpp::get_logger("test"), "check2"); auto nodelay = std::chrono::milliseconds(0); auto timeout = rclcpp::Duration::from_seconds( double(server_timeout_.count()) / 1000); - auto ret = callback_group_executor_.spin_until_future_complete(future_goal_handle_, nodelay); + auto ret = callback_group_executor_->spin_until_future_complete(future_goal_handle_, nodelay); + if (ret != rclcpp::FutureReturnCode::SUCCESS) { if( (node_->now() - time_goal_sent_) > timeout ) @@ -364,7 +376,6 @@ template inline } else { - RCLCPP_INFO(rclcpp::get_logger("test"), "check3"); goal_received_ = true; goal_handle_ = future_goal_handle_.get(); future_goal_handle_ = {}; @@ -379,12 +390,10 @@ template inline if( on_feedback_state_change_ != NodeStatus::RUNNING ) { - RCLCPP_INFO(rclcpp::get_logger("test"), "check4"); cancelGoal(); return on_feedback_state_change_; } // THIRD case: result received, requested a stop - RCLCPP_INFO(rclcpp::get_logger("test"), "check5"); if( result_.code != rclcpp_action::ResultCode::UNKNOWN) { if( result_.code == rclcpp_action::ResultCode::ABORTED ) @@ -416,17 +425,17 @@ template inline template inline void RosActionSharedNode::cancelGoal() { + std::lock_guard lock(action_client_mutex_); auto future_result = action_client_->async_get_result(goal_handle_); auto future_cancel = action_client_->async_cancel_goal(goal_handle_); - - if (callback_group_executor_.spin_until_future_complete(future_cancel, server_timeout_) != + 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]", prev_action_name_.c_str()); } - if (callback_group_executor_.spin_until_future_complete(future_result, server_timeout_) != + if (callback_group_executor_->spin_until_future_complete(future_result, server_timeout_) != rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR( node_->get_logger(), "Failed to get result call failed :( for [%s]", @@ -434,9 +443,19 @@ template inline } } +template +typename std::shared_ptr> RosActionSharedNode::action_client_ = nullptr; + +template +std::shared_ptr RosActionSharedNode::callback_group_executor_ = nullptr; + template bool RosActionSharedNode::shared_resource_initialized = false; template -typename std::shared_ptr> RosActionSharedNode::action_client_ = nullptr; +std::mutex RosActionSharedNode::static_members_mutex_; + +template +std::mutex RosActionSharedNode::action_client_mutex_; + } // namespace BT diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_service_error_code.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_service_error_code.hpp new file mode 100644 index 0000000..10c485a --- /dev/null +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_service_error_code.hpp @@ -0,0 +1,27 @@ +#ifndef BEHAVIORTREE_ROS2__BT_SERVICE_ERROR_CODE_HPP_ +#define BEHAVIORTREE_ROS2__BT_SERVICE_ERROR_CODE_HPP_ + +namespace BT +{ +enum ServiceNodeErrorCode +{ + SERVICE_UNREACHABLE, + SERVICE_TIMEOUT, + INVALID_REQUEST, + SERVICE_ABORTED +}; + +inline const char* toStr(const ServiceNodeErrorCode& err) +{ + switch (err) + { + case SERVICE_UNREACHABLE: return "SERVICE_UNREACHABLE"; + case SERVICE_TIMEOUT: return "SERVICE_TIMEOUT"; + case INVALID_REQUEST: return "INVALID_REQUEST"; + case SERVICE_ABORTED: return "SERVICE_ABORTED"; + } + return nullptr; +} +} // namespace BT + +#endif // BEHAVIORTREE_ROS2__BT_SERVICE_ERROR_CODE_HPP_ diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp index ea63990..77ccc6c 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp @@ -22,30 +22,10 @@ #include "behaviortree_cpp/bt_factory.h" #include "behaviortree_ros2/ros_node_params.hpp" +#include "behaviortree_ros2/bt_service_error_code.hpp" namespace BT { - -enum ServiceNodeErrorCode -{ - SERVICE_UNREACHABLE, - SERVICE_TIMEOUT, - INVALID_REQUEST, - SERVICE_ABORTED -}; - -inline const char* toStr(const ServiceNodeErrorCode& err) -{ - switch (err) - { - case SERVICE_UNREACHABLE: return "SERVICE_UNREACHABLE"; - case SERVICE_TIMEOUT: return "SERVICE_TIMEOUT"; - case INVALID_REQUEST: return "INVALID_REQUEST"; - case SERVICE_ABORTED: return "SERVICE_ABORTED"; - } - return nullptr; -} - /** * @brief Abstract class use to wrap rclcpp::Client<> * diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_service_shared_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_service_shared_node.hpp new file mode 100644 index 0000000..87e681c --- /dev/null +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_service_shared_node.hpp @@ -0,0 +1,338 @@ +// Copyright (c) 2019 Intel Corporation +// Copyright (c) 2023 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. + +#pragma once + +#include +#include +#include +#include +#include +#include "behaviortree_cpp/bt_factory.h" + +#include "behaviortree_ros2/ros_node_params.hpp" +#include "behaviortree_ros2/bt_service_error_code.hpp" + +namespace BT +{ +/** + * @brief Abstract class use to wrap rclcpp::Client<> + * + * For instance, given the type AddTwoInts described in this tutorial: + * https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Writing-A-Simple-Cpp-Service-And-Client.html + * + * the corresponding wrapper would be: + * + * class AddTwoNumbers: public RosServiceSharedNode + * + * RosServiceSharedNode will try to be non-blocking for the entire duration of the call. + * The derived class must reimplement the virtual methods as described below. + * + * The name of the service will be determined as follows: + * + * 1. If a value is passes in the InputPort "service_name", use that + * 2. Otherwise, use the value in RosNodeParams::default_port_value + */ +template +class RosServiceSharedNode : public BT::ActionNodeBase +{ + +public: + // Type definitions + using ServiceClient = typename rclcpp::Client; + using Request = typename ServiceT::Request; + using Response = typename ServiceT::Response; + + /** To register this class into the factory, use: + * + * factory.registerNodeType<>(node_name, params); + */ + explicit RosServiceSharedNode(const std::string & instance_name, + const BT::NodeConfig& conf, + const BT::RosNodeParams& params); + + virtual ~RosServiceSharedNode() = default; + + /** + * @brief Any subclass of RosServiceSharedNode that has ports must implement 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", "__default__placeholder__", "Service name") + }; + 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({}); + } + + NodeStatus tick() override final; + + /// The default halt() implementation. + void halt() override; + + /** setRequest is a callback that allows the user to set + * the request message (ServiceT::Request). + * + * @param request the request to be sent to the service provider. + * + * @return false if the request should not be sent. In that case, + * RosServiceSharedNode::onFailure(INVALID_REQUEST) will be called. + */ + virtual bool setRequest(typename Request::SharedPtr& request) = 0; + + /** Callback invoked when the response is received by the server. + * It is up to the user to define if this returns SUCCESS or FAILURE. + */ + virtual BT::NodeStatus onResponseReceived(const typename Response::SharedPtr& response) = 0; + + /** Callback invoked when something goes wrong; you can override it. + * It must return either SUCCESS or FAILURE. + */ + virtual BT::NodeStatus onFailure(ServiceNodeErrorCode /*error*/) + { + return NodeStatus::FAILURE; + } + +protected: + + std::shared_ptr node_; + std::string prev_service_name_; + bool service_name_may_change_ = false; + const std::chrono::milliseconds service_timeout_; + const std::chrono::milliseconds wait_for_service_timeout_; + static typename std::shared_ptr service_client_; + static bool shared_resource_initialized; +private: + rclcpp::CallbackGroup::SharedPtr callback_group_; + static std::shared_ptr callback_group_executor_; + static std::mutex mutex_; + + std::shared_future future_response_; + + rclcpp::Time time_request_sent_; + NodeStatus on_feedback_state_change_; + bool response_received_; + typename Response::SharedPtr response_; + + bool createClient(const std::string &service_name); +}; + +//---------------------------------------------------------------- +//---------------------- DEFINITIONS ----------------------------- +//---------------------------------------------------------------- + +template inline + RosServiceSharedNode::RosServiceSharedNode(const std::string & instance_name, + const NodeConfig &conf, + const RosNodeParams& params): + BT::ActionNodeBase(instance_name, conf), + node_(params.nh), + service_timeout_(params.server_timeout), + wait_for_service_timeout_(params.wait_for_server_timeout) +{ + // check port remapping + auto portIt = config().input_ports.find("service_name"); + if(portIt != config().input_ports.end()) + { + const std::string& bb_service_name = portIt->second; + + if(bb_service_name.empty() || bb_service_name == "__default__placeholder__") + { + if(params.default_port_value.empty()) { + throw std::logic_error( + "Both [service_name] in the InputPort and the RosNodeParams are empty."); + } + else { + createClient(params.default_port_value); + } + } + else if(!isBlackboardPointer(bb_service_name)) + { + // If the content of the port "service_name" is not + // a pointer to the blackboard, but a static string, we can + // create the client in the constructor. + createClient(bb_service_name); + } + else { + service_name_may_change_ = true; + // createClient will be invoked in the first tick(). + } + } + else { + + if(params.default_port_value.empty()) { + throw std::logic_error( + "Both [service_name] in the InputPort and the RosNodeParams are empty."); + } + else { + createClient(params.default_port_value); + } + } +} + +template inline + bool RosServiceSharedNode::createClient(const std::string& service_name) +{ + std::lock_guard lock(mutex_); + if(service_name.empty()) + { + throw RuntimeError("service_name is empty"); + } + + if (!shared_resource_initialized) { + callback_group_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + if (callback_group_executor_ == nullptr) { + callback_group_executor_ = std::make_shared(); + } + callback_group_executor_->add_callback_group(callback_group_, node_->get_node_base_interface()); + service_client_ = node_->create_client(service_name, rmw_qos_profile_services_default, callback_group_); + prev_service_name_ = service_name; + shared_resource_initialized = true; + } + + + bool found = service_client_->wait_for_service(wait_for_service_timeout_); + if(!found) + { + RCLCPP_ERROR(node_->get_logger(), "%s: Service with name '%s' is not reachable.", + name().c_str(), prev_service_name_.c_str()); + } + return found; +} + +template inline + NodeStatus RosServiceSharedNode::tick() +{ + // First, check if the service_client_ is valid and that the name of the + // service_name in the port didn't change. + // otherwise, create a new client + if(!service_client_ || (status() == NodeStatus::IDLE && service_name_may_change_)) + { + std::string service_name; + getInput("service_name", service_name); + if(prev_service_name_ != service_name) + { + createClient(service_name); + } + } + + auto CheckStatus = [](NodeStatus status) + { + if( !isStatusCompleted(status) ) + { + throw std::logic_error("RosServiceSharedNode: the callback must return either SUCCESS or FAILURE"); + } + return status; + }; + + // first step to be done only at the beginning of the Action + if (status() == BT::NodeStatus::IDLE) + { + setStatus(NodeStatus::RUNNING); + + response_received_ = false; + future_response_ = {}; + on_feedback_state_change_ = NodeStatus::RUNNING; + response_ = {}; + + typename Request::SharedPtr request = std::make_shared(); + + if( !setRequest(request) ) + { + return CheckStatus( onFailure(INVALID_REQUEST) ); + } + + future_response_ = service_client_->async_send_request(request).share(); + time_request_sent_ = node_->now(); + + return NodeStatus::RUNNING; + } + + if (status() == NodeStatus::RUNNING) + { + std::lock_guard lock(mutex_); + callback_group_executor_->spin_some(); + + // FIRST case: check if the goal request has a timeout + if( !response_received_ ) + { + auto const nodelay = std::chrono::milliseconds(0); + auto const timeout = rclcpp::Duration::from_seconds( double(service_timeout_.count()) / 1000); + + auto ret = callback_group_executor_->spin_until_future_complete(future_response_, nodelay); + + if (ret != rclcpp::FutureReturnCode::SUCCESS) + { + if( (node_->now() - time_request_sent_) > timeout ) + { + return CheckStatus( onFailure(SERVICE_TIMEOUT) ); + } + else{ + return NodeStatus::RUNNING; + } + } + else + { + response_received_ = true; + response_ = future_response_.get(); + future_response_ = {}; + + if (!response_) { + throw std::runtime_error("Request was rejected by the service"); + } + } + } + + // SECOND case: response received + return CheckStatus( onResponseReceived( response_ ) ); + } + return NodeStatus::RUNNING; +} + +template inline + void RosServiceSharedNode::halt() +{ + if( status() == NodeStatus::RUNNING ) + { + resetStatus(); + } +} + +template +bool RosServiceSharedNode::shared_resource_initialized = false; + +template +typename std::shared_ptr> RosServiceSharedNode::service_client_ = nullptr; + +template +std::mutex RosServiceSharedNode::mutex_; + +template +std::shared_ptr RosServiceSharedNode::callback_group_executor_ = nullptr; +} // namespace BT + diff --git a/btcpp_ros2_samples/CMakeLists.txt b/btcpp_ros2_samples/CMakeLists.txt index 645781a..e440cab 100644 --- a/btcpp_ros2_samples/CMakeLists.txt +++ b/btcpp_ros2_samples/CMakeLists.txt @@ -70,6 +70,9 @@ ament_target_dependencies(add_two_ints_server ${THIS_PACKAGE_DEPS}) add_executable(service_test src/service_test.cpp) ament_target_dependencies(service_test ${THIS_PACKAGE_DEPS}) +add_executable(service_shared_test src/service_shared_test.cpp) +ament_target_dependencies(service_shared_test ${THIS_PACKAGE_DEPS}) + ###################################################### # INSTALL @@ -85,6 +88,7 @@ install(TARGETS publisher_shared_test add_two_ints_server service_test + service_shared_test DESTINATION lib/${PROJECT_NAME} ) diff --git a/btcpp_ros2_samples/src/service_shared_test.cpp b/btcpp_ros2_samples/src/service_shared_test.cpp new file mode 100644 index 0000000..8c4469c --- /dev/null +++ b/btcpp_ros2_samples/src/service_shared_test.cpp @@ -0,0 +1,85 @@ +#include "behaviortree_ros2/bt_service_shared_node.hpp" +#include "btcpp_ros2_interfaces/srv/add_two_ints.hpp" + +using namespace BT; + +class AddTwoIntsClient: public RosServiceSharedNode +{ +public: + AddTwoIntsClient(const std::string& name, + const NodeConfig& conf, + const RosNodeParams& params) + : RosServiceSharedNode(name, conf, params) + {} + + static PortsList providedPorts() + { + BT::PortsList addition; + BT::PortsList basic = { + InputPort("a", 12, "first integer to add"), + InputPort("b", 12, "second integer to add") + }; + basic.insert(addition.begin(), addition.end()); + return providedBasicPorts(basic); + } + + bool setRequest(Request::SharedPtr& req) override + { + getInput("a", a); + getInput("b", b); + req->a = a; + req->b = b; + + return true; + } + + virtual BT::NodeStatus onResponseReceived(const Response::SharedPtr& res) override + { + RCLCPP_INFO(rclcpp::get_logger("test"), "pointer: %p", reinterpret_cast(service_client_.get())); + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Result of %d + %d = %ld", + a, b, res->sum); + return BT::NodeStatus::SUCCESS; + } + + virtual BT::NodeStatus onFailure(ServiceNodeErrorCode error_code) override + { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "AddTwoIntsClient failed with error code %d", error_code); + return BT::NodeStatus::FAILURE; + } +private: + int a; + int b; +}; + + // 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("service_test"); + + BehaviorTreeFactory factory; + + RosNodeParams params; + params.nh = nh; + params.default_port_value = "btcpp_service"; + factory.registerNodeType("AddTwoIntsClient", params); + + auto tree = factory.createTreeFromText(xml_text); + + while(rclcpp::ok()) + { + tree.tickWhileRunning(); + } + return 0; +} From 8490f4eb2eb659611ddc314ddbb08b4db484ef82 Mon Sep 17 00:00:00 2001 From: Rebecca Williams Date: Mon, 29 Jan 2024 15:17:18 +0000 Subject: [PATCH 07/18] catch ctrl c --- .../behaviortree_ros2/bt_action_node.hpp | 5 +++ .../bt_action_shared_node.hpp | 43 ++++++++++++++++--- btcpp_ros2_samples/src/sleep_client.cpp | 9 ++-- 3 files changed, 47 insertions(+), 10 deletions(-) diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp index a67e7cf..f568012 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp @@ -249,6 +249,11 @@ template inline template inline NodeStatus RosActionNode::tick() { + if (!rclcpp::ok()) { + RCLCPP_INFO(rclcpp::get_logger("test"), "Node is not ok"); + halt(); + return NodeStatus::FAILURE; + } // First, check if the action_client_ is valid and that the name of the // action_name in the port didn't change. // otherwise, create a new client diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_action_shared_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_action_shared_node.hpp index e55f4e0..ed2956e 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_action_shared_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_action_shared_node.hpp @@ -70,7 +70,7 @@ class RosActionSharedNode : public BT::ActionNodeBase const BT::NodeConfig& conf, const RosNodeParams& params); - virtual ~RosActionSharedNode() = default; + virtual ~RosActionSharedNode(); /** * @brief Any subclass of RosActionSharedNode that has ports must implement a @@ -146,6 +146,7 @@ class RosActionSharedNode : public BT::ActionNodeBase std::shared_ptr node_; std::string prev_action_name_; + std::string action_name_; bool action_name_may_change_ = false; const std::chrono::milliseconds server_timeout_; const std::chrono::milliseconds wait_for_server_timeout_; @@ -158,8 +159,6 @@ class RosActionSharedNode : public BT::ActionNodeBase rclcpp::CallbackGroup::SharedPtr callback_group_; static std::shared_ptr callback_group_executor_; - static std::mutex static_members_mutex_; - std::shared_future future_goal_handle_; typename GoalHandle::SharedPtr goal_handle_; @@ -230,15 +229,48 @@ template inline } } +template +RosActionSharedNode::~RosActionSharedNode() +{ + RCLCPP_INFO(node_->get_logger(), "Destroying Action server for %s", action_name_.c_str()); + if (action_client_ && goal_handle_) { + auto state = goal_handle_->get_status(); + if (state == rclcpp_action::GoalStatus::STATUS_ACCEPTED || + state == rclcpp_action::GoalStatus::STATUS_EXECUTING) { + try { + auto future_cancel = action_client_->async_cancel_goal(goal_handle_); + if (callback_group_executor_) { + callback_group_executor_->spin_some(); + } + } catch (const std::exception& e) { + RCLCPP_ERROR(node_->get_logger(), "Exception during goal cancellation in destructor: %s", e.what()); + } + } + } + + + if (callback_group_executor_) { + callback_group_executor_.reset(); + } + + if (action_client_) { + action_client_.reset(); + } + + RCLCPP_INFO(node_->get_logger(), "Action server %s destroyed", action_name_.c_str()); +} + template inline bool RosActionSharedNode::createClient(const std::string& action_name) { - std::lock_guard lock(static_members_mutex_); + std::lock_guard lock(action_client_mutex_); if (action_name.empty()) { throw RuntimeError("action_name is empty"); } + this->action_name_ = action_name; + if (!shared_resource_initialized) { RCLCPP_INFO(node_->get_logger(), "Creating action client for %s", action_name.c_str()); @@ -452,9 +484,6 @@ std::shared_ptr RosActionSharedNode bool RosActionSharedNode::shared_resource_initialized = false; -template -std::mutex RosActionSharedNode::static_members_mutex_; - template std::mutex RosActionSharedNode::action_client_mutex_; diff --git a/btcpp_ros2_samples/src/sleep_client.cpp b/btcpp_ros2_samples/src/sleep_client.cpp index 2acf6e9..9839b56 100644 --- a/btcpp_ros2_samples/src/sleep_client.cpp +++ b/btcpp_ros2_samples/src/sleep_client.cpp @@ -44,17 +44,19 @@ class PrintValue : public BT::SyncActionNode static const char* xml_text = R"( + - + + )"; @@ -80,9 +82,10 @@ int main(int argc, char **argv) auto tree = factory.createTreeFromText(xml_text); - for(int i=0; i<5; i++){ + // while(rclcpp::ok()) + // { tree.tickWhileRunning(); - } + // } return 0; } From 921976d0cfda229c5a856260cb8d23b1b84d64e6 Mon Sep 17 00:00:00 2001 From: Rebecca Williams Date: Tue, 30 Jan 2024 10:40:04 +0000 Subject: [PATCH 08/18] remove log --- .../include/behaviortree_ros2/bt_action_node.hpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp index f568012..241bb37 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp @@ -249,11 +249,12 @@ template inline template inline NodeStatus RosActionNode::tick() { - if (!rclcpp::ok()) { - RCLCPP_INFO(rclcpp::get_logger("test"), "Node is not ok"); + if (!rclcpp::ok()) + { halt(); return NodeStatus::FAILURE; } + // First, check if the action_client_ is valid and that the name of the // action_name in the port didn't change. // otherwise, create a new client From dee2cdb60af294bfba482f9198a468d98327337d Mon Sep 17 00:00:00 2001 From: Rebecca Williams Date: Tue, 30 Jan 2024 11:12:31 +0000 Subject: [PATCH 09/18] add checking if rclcpp if ok to ticks --- .../bt_action_shared_node.hpp | 25 +++++++++++-------- .../behaviortree_ros2/bt_service_node.hpp | 5 ++++ .../bt_service_shared_node.hpp | 6 +++++ btcpp_ros2_samples/src/sleep_client.cpp | 6 ++--- .../src/sleep_client_shared.cpp | 12 +++++++-- 5 files changed, 38 insertions(+), 16 deletions(-) diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_action_shared_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_action_shared_node.hpp index ed2956e..5b1a041 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_action_shared_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_action_shared_node.hpp @@ -145,8 +145,7 @@ class RosActionSharedNode : public BT::ActionNodeBase protected: std::shared_ptr node_; - std::string prev_action_name_; - std::string action_name_; + std::string current_action_name_; bool action_name_may_change_ = false; const std::chrono::milliseconds server_timeout_; const std::chrono::milliseconds wait_for_server_timeout_; @@ -232,7 +231,7 @@ template inline template RosActionSharedNode::~RosActionSharedNode() { - RCLCPP_INFO(node_->get_logger(), "Destroying Action server for %s", action_name_.c_str()); + RCLCPP_INFO(node_->get_logger(), "Destroying Action server for %s", current_action_name_.c_str()); if (action_client_ && goal_handle_) { auto state = goal_handle_->get_status(); if (state == rclcpp_action::GoalStatus::STATUS_ACCEPTED || @@ -257,7 +256,7 @@ RosActionSharedNode::~RosActionSharedNode() action_client_.reset(); } - RCLCPP_INFO(node_->get_logger(), "Action server %s destroyed", action_name_.c_str()); + RCLCPP_INFO(node_->get_logger(), "Action server %s destroyed", current_action_name_.c_str()); } template inline @@ -269,8 +268,6 @@ bool RosActionSharedNode::createClient(const std::string& action_name) throw RuntimeError("action_name is empty"); } - this->action_name_ = action_name; - if (!shared_resource_initialized) { RCLCPP_INFO(node_->get_logger(), "Creating action client for %s", action_name.c_str()); @@ -282,7 +279,7 @@ bool RosActionSharedNode::createClient(const std::string& action_name) callback_group_executor_->add_callback_group(callback_group_, node_->get_node_base_interface()); action_client_ = rclcpp_action::create_client(node_, action_name, callback_group_); - prev_action_name_ = action_name; + current_action_name_ = action_name; shared_resource_initialized = true; } @@ -290,7 +287,7 @@ bool RosActionSharedNode::createClient(const std::string& action_name) if(!found) { RCLCPP_ERROR(node_->get_logger(), "%s: Action server with name '%s' is not reachable.", - name().c_str(), prev_action_name_.c_str()); + name().c_str(), current_action_name_.c_str()); } return found; } @@ -298,6 +295,12 @@ bool RosActionSharedNode::createClient(const std::string& action_name) template inline NodeStatus RosActionSharedNode::tick() { + if (!rclcpp::ok()) + { + halt(); + return NodeStatus::FAILURE; + } + // First, check if the action_client_ is valid and that the name of the // action_name in the port didn't change. // otherwise, create a new client @@ -306,7 +309,7 @@ template inline RCLCPP_INFO(node_->get_logger(), "Client not initialized or action_name changed"); std::string action_name; getInput("action_name", action_name); - if(prev_action_name_ != action_name) + if(current_action_name_ != action_name) { createClient(action_name); } @@ -464,14 +467,14 @@ template inline rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR( node_->get_logger(), "Failed to cancel action server for [%s]", - prev_action_name_.c_str()); + current_action_name_.c_str()); } if (callback_group_executor_->spin_until_future_complete(future_result, server_timeout_) != rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR( node_->get_logger(), "Failed to get result call failed :( for [%s]", - prev_action_name_.c_str()); + current_action_name_.c_str()); } } diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp index 77ccc6c..8cd6338 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp @@ -219,6 +219,11 @@ template inline template inline NodeStatus RosServiceNode::tick() { + if (!rclcpp::ok()) + { + halt(); + return NodeStatus::FAILURE; + } // First, check if the service_client_ is valid and that the name of the // service_name in the port didn't change. // otherwise, create a new client diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_service_shared_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_service_shared_node.hpp index 87e681c..5127873 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_service_shared_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_service_shared_node.hpp @@ -228,6 +228,12 @@ template inline template inline NodeStatus RosServiceSharedNode::tick() { + if (!rclcpp::ok()) + { + halt(); + return NodeStatus::FAILURE; + } + // First, check if the service_client_ is valid and that the name of the // service_name in the port didn't change. // otherwise, create a new client diff --git a/btcpp_ros2_samples/src/sleep_client.cpp b/btcpp_ros2_samples/src/sleep_client.cpp index 9839b56..5899e40 100644 --- a/btcpp_ros2_samples/src/sleep_client.cpp +++ b/btcpp_ros2_samples/src/sleep_client.cpp @@ -82,10 +82,10 @@ int main(int argc, char **argv) auto tree = factory.createTreeFromText(xml_text); - // while(rclcpp::ok()) - // { + while(rclcpp::ok()) + { tree.tickWhileRunning(); - // } + } return 0; } diff --git a/btcpp_ros2_samples/src/sleep_client_shared.cpp b/btcpp_ros2_samples/src/sleep_client_shared.cpp index 09f9bfe..12acd0b 100644 --- a/btcpp_ros2_samples/src/sleep_client_shared.cpp +++ b/btcpp_ros2_samples/src/sleep_client_shared.cpp @@ -44,12 +44,19 @@ class PrintValue : public BT::SyncActionNode static const char* xml_text = R"( + - + + + + + + + )"; @@ -82,7 +89,8 @@ int main(int argc, char **argv) auto tree = factory.createTreeFromText(xml_text); - for(int i=0; i<5; i++){ + while(rclcpp::ok()) + { tree.tickWhileRunning(); } From d4a573d1d8870c1d1adebb75ac243f3b3d23f202 Mon Sep 17 00:00:00 2001 From: Rebecca Williams Date: Tue, 30 Jan 2024 11:19:57 +0000 Subject: [PATCH 10/18] remove comments --- .vscode/settings.json | 59 ++++ .../behaviortree_ros2/bt_topic_pub_node.hpp | 8 +- .../bt_topic_pub_shared_node.hpp | 4 + .../behaviortree_ros2/bt_topic_sub_node.hpp | 22 +- .../bt_topic_sub_shared_node.hpp | 312 ------------------ btcpp_ros2_samples/CMakeLists.txt | 4 - .../src/publisher_test_shared.cpp | 1 - .../src/service_shared_test.cpp | 1 - btcpp_ros2_samples/src/service_test.cpp | 1 - btcpp_ros2_samples/src/sleep_action.cpp | 3 - .../src/sleep_action_shared.cpp | 3 - .../src/sleep_client_shared.cpp | 9 +- .../src/subscriber_shared_test.cpp | 64 ---- btcpp_ros2_samples/src/subscriber_test.cpp | 1 - 14 files changed, 83 insertions(+), 409 deletions(-) create mode 100644 .vscode/settings.json delete mode 100644 behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_shared_node.hpp delete mode 100644 btcpp_ros2_samples/src/subscriber_shared_test.cpp diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..165f327 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,59 @@ +{ + "files.associations": { + "array": "cpp", + "atomic": "cpp", + "bit": "cpp", + "*.tcc": "cpp", + "cctype": "cpp", + "chrono": "cpp", + "clocale": "cpp", + "cmath": "cpp", + "codecvt": "cpp", + "compare": "cpp", + "concepts": "cpp", + "cstdarg": "cpp", + "cstddef": "cpp", + "cstdint": "cpp", + "cstdio": "cpp", + "cstdlib": "cpp", + "ctime": "cpp", + "cwchar": "cpp", + "cwctype": "cpp", + "deque": "cpp", + "string": "cpp", + "unordered_map": "cpp", + "vector": "cpp", + "exception": "cpp", + "algorithm": "cpp", + "functional": "cpp", + "iterator": "cpp", + "memory": "cpp", + "memory_resource": "cpp", + "numeric": "cpp", + "random": "cpp", + "ratio": "cpp", + "string_view": "cpp", + "system_error": "cpp", + "tuple": "cpp", + "type_traits": "cpp", + "utility": "cpp", + "initializer_list": "cpp", + "iomanip": "cpp", + "iosfwd": "cpp", + "istream": "cpp", + "limits": "cpp", + "new": "cpp", + "numbers": "cpp", + "ostream": "cpp", + "semaphore": "cpp", + "sstream": "cpp", + "stdexcept": "cpp", + "stop_token": "cpp", + "streambuf": "cpp", + "thread": "cpp", + "typeinfo": "cpp", + "iostream": "cpp", + "condition_variable": "cpp", + "mutex": "cpp" + } +} diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_pub_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_pub_node.hpp index e0d1877..00ea664 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_pub_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_pub_node.hpp @@ -109,7 +109,7 @@ template inline const RosNodeParams& params) : BT::ConditionNode(instance_name, conf), node_(params.nh) -{ +{ // check port remapping auto portIt = config().input_ports.find("topic_name"); if(portIt != config().input_ports.end()) @@ -156,7 +156,7 @@ template inline { throw RuntimeError("topic_name is empty"); } - + publisher_ = node_->create_publisher(topic_name, 1); prev_topic_name_ = topic_name; return true; @@ -165,6 +165,10 @@ template inline template inline NodeStatus RosTopicPubNode::tick() { + if (!rclcpp::ok()) + { + return NodeStatus::FAILURE; + } // First, check if the subscriber_ is valid and that the name of the // topic_name in the port didn't change. // otherwise, create a new subscriber diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_pub_shared_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_pub_shared_node.hpp index 29e9c99..c95f2aa 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_pub_shared_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_pub_shared_node.hpp @@ -168,6 +168,10 @@ template inline template inline NodeStatus RosTopicPubSharedNode::tick() { + if (!rclcpp::ok()) + { + return NodeStatus::FAILURE; + } // First, check if the subscriber_ is valid and that the name of the // topic_name in the port didn't change. // otherwise, create a new subscriber diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp index 311594b..e520192 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp @@ -49,13 +49,13 @@ class RosTopicSubNode : public BT::ConditionNode // Type definitions using Subscriber = typename rclcpp::Subscription; - protected: + protected: struct SubscriberInstance { void init(std::shared_ptr node, const std::string& topic_name) { // create a callback group for this particular instance - callback_group = + callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); callback_group_executor.add_callback_group( callback_group, node->get_node_base_interface()); @@ -64,7 +64,7 @@ class RosTopicSubNode : public BT::ConditionNode option.callback_group = callback_group; // The callback will broadcast to all the instances of RosTopicSubNode - auto callback = [this](const std::shared_ptr msg) + auto callback = [this](const std::shared_ptr msg) { broadcaster(msg); }; @@ -117,7 +117,7 @@ class RosTopicSubNode : public BT::ConditionNode const BT::NodeConfig& conf, const RosNodeParams& params); - virtual ~RosTopicSubNode() + virtual ~RosTopicSubNode() { signal_connection_.disconnect(); // remove the subscribers from the static registry when the ALL the @@ -168,7 +168,7 @@ class RosTopicSubNode : public BT::ConditionNode * * @param last_msg the latest message received, since the last tick. * Will be empty if no new message received. - * + * * @return the new status of the Node, based on last_msg */ virtual NodeStatus onTick(const std::shared_ptr& last_msg) = 0; @@ -188,7 +188,7 @@ template inline const RosNodeParams& params) : BT::ConditionNode(instance_name, conf), node_(params.nh) -{ +{ // check port remapping auto portIt = config().input_ports.find("topic_name"); if(portIt != config().input_ports.end()) @@ -252,7 +252,7 @@ template inline sub_instance_ = it->second; sub_instance_->init(node_, topic_name); - RCLCPP_INFO(logger(), + RCLCPP_INFO(logger(), "Node [%s] created Subscriber to topic [%s]", name().c_str(), topic_name.c_str() ); } @@ -273,6 +273,10 @@ template inline template inline NodeStatus RosTopicSubNode::tick() { + if (!rclcpp::ok()) + { + return NodeStatus::FAILURE; + } // First, check if the subscriber_ is valid and that the name of the // topic_name in the port didn't change. // otherwise, create a new subscriber @@ -280,14 +284,14 @@ template inline { std::string topic_name; getInput("topic_name", topic_name); - createSubscriber(topic_name); + createSubscriber(topic_name); } auto CheckStatus = [](NodeStatus status) { if( !isStatusCompleted(status) ) { - throw std::logic_error("RosTopicSubNode: the callback must return" + throw std::logic_error("RosTopicSubNode: the callback must return" "either SUCCESS or FAILURE"); } return status; diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_shared_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_shared_node.hpp deleted file mode 100644 index 2888b82..0000000 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_shared_node.hpp +++ /dev/null @@ -1,312 +0,0 @@ -// Copyright (c) 2023 Davide Faconti, Unmanned Life -// -// 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 -#include -#include -#include "behaviortree_cpp/condition_node.h" -#include "behaviortree_cpp/bt_factory.h" - -#include "behaviortree_ros2/ros_node_params.hpp" -#include - -namespace BT -{ - - -/** - * @brief Abstract class to wrap a Topic subscriber. - * Considering the example in the tutorial: - * https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Writing-A-Simple-Cpp-Publisher-And-Subscriber.html - * - * The corresponding wrapper would be: - * - * class SubscriberNode: RosTopicSubSharedNode - * - * The name of the topic will be determined as follows: - * - * 1. If a value is passes in the InputPort "topic_name", use that - * 2. Otherwise, use the value in RosNodeParams::default_port_value - */ -template -class RosTopicSubSharedNode : public BT::ConditionNode -{ - public: - // Type definitions - using Subscriber = typename rclcpp::Subscription; - - protected: - struct SubscriberInstance - { - void init(std::shared_ptr node, const std::string& topic_name) - { - if (!shared_resource_initialized) { - // create a callback group for this particular instance - callback_group = - node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); - callback_group_executor.add_callback_group( - callback_group, node->get_node_base_interface()); - - rclcpp::SubscriptionOptions option; - option.callback_group = callback_group; - - // The callback will broadcast to all the instances of RosTopicSubSharedNode - auto callback = [this](const std::shared_ptr msg) - { - broadcaster(msg); - }; - subscriber = node->create_subscription(topic_name, 1, callback, option); - } - } - - std::shared_ptr subscriber; - rclcpp::CallbackGroup::SharedPtr callback_group; - rclcpp::executors::SingleThreadedExecutor callback_group_executor; - boost::signals2::signal)> broadcaster; - - - }; - - static std::mutex& registryMutex() - { - static std::mutex sub_mutex; - return sub_mutex; - } - - // contains the fully-qualified name of the node and the name of the topic - static std::unordered_map>& getRegistry() - { - static std::unordered_map> subscribers_registry; - return subscribers_registry; - } - - std::shared_ptr node_; - std::shared_ptr sub_instance_ = nullptr; - std::shared_ptr last_msg_; - std::string topic_name_; - boost::signals2::connection signal_connection_; - std::string subscriber_key_; - static bool shared_resource_initialized; - - rclcpp::Logger logger() - { - return node_->get_logger(); - } - - public: - - /** 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 RosTopicSubSharedNode(const std::string & instance_name, - const BT::NodeConfig& conf, - const RosNodeParams& params); - - virtual ~RosTopicSubSharedNode() - { - signal_connection_.disconnect(); - // remove the subscribers from the static registry when the ALL the - // instances of RosTopicSubSharedNode are destroyed (i.e., the tree is destroyed) - if(sub_instance_) - { - sub_instance_.reset(); - std::unique_lock lk(registryMutex()); - auto& registry = getRegistry(); - auto it = registry.find(subscriber_key_); - // when the reference count is 1, means that the only instance is owned by the - // registry itself. Delete it - if(it != registry.end() && it->second.use_count() <= 1) - { - registry.erase(it); - RCLCPP_INFO(logger(), "Remove subscriber [%s]", topic_name_.c_str() ); - } - } - } - - /** - * @brief Any subclass of RosTopicNode 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("topic_name", "__default__placeholder__", "Topic name") - }; - 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({}); - } - - NodeStatus tick() override final; - - /** Callback invoked in the tick. You must return either SUCCESS of FAILURE - * - * @param last_msg the latest message received, since the last tick. - * Will be empty if no new message received. - * - * @return the new status of the Node, based on last_msg - */ - virtual NodeStatus onTick(const std::shared_ptr& last_msg) = 0; - -private: - - bool createSubscriber(const std::string& topic_name); -}; - -//---------------------------------------------------------------- -//---------------------- DEFINITIONS ----------------------------- -//---------------------------------------------------------------- - -template inline - RosTopicSubSharedNode::RosTopicSubSharedNode(const std::string & instance_name, - const NodeConfig &conf, - const RosNodeParams& params) - : BT::ConditionNode(instance_name, conf), - node_(params.nh) -{ - // check port remapping - auto portIt = config().input_ports.find("topic_name"); - if(portIt != config().input_ports.end()) - { - const std::string& bb_topic_name = portIt->second; - - if(bb_topic_name.empty() || bb_topic_name == "__default__placeholder__") - { - if(params.default_port_value.empty()) { - throw std::logic_error( - "Both [topic_name] in the InputPort and the RosNodeParams are empty."); - } - else { - createSubscriber(params.default_port_value); - } - } - else if(!isBlackboardPointer(bb_topic_name)) - { - // If the content of the port "topic_name" is not - // a pointer to the blackboard, but a static string, we can - // create the client in the constructor. - createSubscriber(bb_topic_name); - } - else { - // do nothing - // createSubscriber will be invoked in the first tick(). - } - } - else { - if(params.default_port_value.empty()) { - throw std::logic_error( - "Both [topic_name] in the InputPort and the RosNodeParams are empty."); - } - else { - createSubscriber(params.default_port_value); - } - } -} - -template inline - bool RosTopicSubSharedNode::createSubscriber(const std::string& topic_name) -{ - if(topic_name.empty()) - { - throw RuntimeError("topic_name is empty"); - } - if(sub_instance_) - { - throw RuntimeError("Can't call createSubscriber more than once"); - } - - // find SubscriberInstance in the registry - std::unique_lock lk(registryMutex()); - subscriber_key_ = std::string(node_->get_fully_qualified_name()) + "/" + topic_name; - - auto& registry = getRegistry(); - auto it = registry.find(subscriber_key_); - if(it == registry.end()) - { - it = registry.insert( {subscriber_key_, std::make_shared() }).first; - sub_instance_ = it->second; - sub_instance_->init(node_, topic_name); - - RCLCPP_INFO(logger(), - "Node [%s] created Subscriber to topic [%s]", - name().c_str(), topic_name.c_str() ); - } - else { - sub_instance_ = it->second; - } - - - // add "this" as received of the broadcaster - signal_connection_ = sub_instance_->broadcaster.connect( - [this](const std::shared_ptr msg) { last_msg_ = msg; } ); - - topic_name_ = topic_name; - return true; -} - - -template inline - NodeStatus RosTopicSubSharedNode::tick() -{ - // First, check if the subscriber_ is valid and that the name of the - // topic_name in the port didn't change. - // otherwise, create a new subscriber - if(!sub_instance_) - { - std::string topic_name; - getInput("topic_name", topic_name); - createSubscriber(topic_name); - } - - auto CheckStatus = [](NodeStatus status) - { - if( !isStatusCompleted(status) ) - { - throw std::logic_error("RosTopicSubSharedNode: the callback must return" - "either SUCCESS or FAILURE"); - } - return status; - }; - sub_instance_->callback_group_executor.spin_some(); - auto status = CheckStatus (onTick(last_msg_)); - last_msg_ = nullptr; - - return status; -} - -template -bool RosTopicSubSharedNode::shared_resource_initialized = false; - -// template -// typename std::shared_ptr> RosTopicPubSharedNode::publisher_ = nullptr; - -} // namespace BT - diff --git a/btcpp_ros2_samples/CMakeLists.txt b/btcpp_ros2_samples/CMakeLists.txt index e440cab..c1be735 100644 --- a/btcpp_ros2_samples/CMakeLists.txt +++ b/btcpp_ros2_samples/CMakeLists.txt @@ -51,9 +51,6 @@ ament_target_dependencies(sleep_server ${THIS_PACKAGE_DEPS}) add_executable(subscriber_test src/subscriber_test.cpp) ament_target_dependencies(subscriber_test ${THIS_PACKAGE_DEPS}) -add_executable(subscriber_shared_test src/subscriber_shared_test.cpp) -ament_target_dependencies(subscriber_shared_test ${THIS_PACKAGE_DEPS}) - ###################################################### # Build publisher_tests add_executable(publisher_test src/publisher_test.cpp) @@ -83,7 +80,6 @@ install(TARGETS sleep_server sleep_plugin subscriber_test - subscriber_shared_test publisher_test publisher_shared_test add_two_ints_server diff --git a/btcpp_ros2_samples/src/publisher_test_shared.cpp b/btcpp_ros2_samples/src/publisher_test_shared.cpp index 7e98028..3a6072f 100644 --- a/btcpp_ros2_samples/src/publisher_test_shared.cpp +++ b/btcpp_ros2_samples/src/publisher_test_shared.cpp @@ -26,7 +26,6 @@ class SendString: public RosTopicPubSharedNode(publisher_.get())); std::string user_message = ""; getInput("message", user_message); msg.data = user_message; diff --git a/btcpp_ros2_samples/src/service_shared_test.cpp b/btcpp_ros2_samples/src/service_shared_test.cpp index 8c4469c..a22d148 100644 --- a/btcpp_ros2_samples/src/service_shared_test.cpp +++ b/btcpp_ros2_samples/src/service_shared_test.cpp @@ -35,7 +35,6 @@ class AddTwoIntsClient: public RosServiceSharedNode(service_client_.get())); RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Result of %d + %d = %ld", a, b, res->sum); return BT::NodeStatus::SUCCESS; diff --git a/btcpp_ros2_samples/src/service_test.cpp b/btcpp_ros2_samples/src/service_test.cpp index 4a574f1..93235fe 100644 --- a/btcpp_ros2_samples/src/service_test.cpp +++ b/btcpp_ros2_samples/src/service_test.cpp @@ -35,7 +35,6 @@ class AddTwoIntsClient: public RosServiceNode(service_client_.get())); RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Result of %d + %d = %ld", a, b, res->sum); return BT::NodeStatus::SUCCESS; diff --git a/btcpp_ros2_samples/src/sleep_action.cpp b/btcpp_ros2_samples/src/sleep_action.cpp index 1754a54..284764c 100644 --- a/btcpp_ros2_samples/src/sleep_action.cpp +++ b/btcpp_ros2_samples/src/sleep_action.cpp @@ -12,8 +12,6 @@ NodeStatus SleepAction::onResultReceived(const RosActionNode::WrappedResult &wr) { RCLCPP_INFO( node_->get_logger(), "%s: onResultReceived. Done = %s", name().c_str(), wr.result->done ? "true" : "false" ); - RCLCPP_INFO(rclcpp::get_logger("test"), " %s action pointer: %p", name().c_str(), reinterpret_cast(action_client_.get())); - return wr.result->done ? NodeStatus::SUCCESS : NodeStatus::FAILURE; } @@ -26,7 +24,6 @@ NodeStatus SleepAction::onFailure(ActionNodeErrorCode error) void SleepAction::onHalt() { RCLCPP_INFO( node_->get_logger(), "%s: onHalt", name().c_str() ); - RCLCPP_INFO(rclcpp::get_logger("test"), " %s action pointer: %p", name().c_str(), reinterpret_cast(action_client_.get())); } // Plugin registration. diff --git a/btcpp_ros2_samples/src/sleep_action_shared.cpp b/btcpp_ros2_samples/src/sleep_action_shared.cpp index 5b271cd..af439bd 100644 --- a/btcpp_ros2_samples/src/sleep_action_shared.cpp +++ b/btcpp_ros2_samples/src/sleep_action_shared.cpp @@ -13,7 +13,6 @@ NodeStatus SleepAction::onResultReceived(const RosActionSharedNode::WrappedResul { RCLCPP_INFO( node_->get_logger(), "%s: onResultReceived. Done = %s", name().c_str(), wr.result->done ? "true" : "false" ); - RCLCPP_INFO(rclcpp::get_logger("test"), " %s action pointer: %p", name().c_str(), reinterpret_cast(action_client_.get())); return wr.result->done ? NodeStatus::SUCCESS : NodeStatus::FAILURE; } @@ -21,14 +20,12 @@ NodeStatus SleepAction::onResultReceived(const RosActionSharedNode::WrappedResul NodeStatus SleepAction::onFailure(ActionNodeErrorCode error) { RCLCPP_ERROR( node_->get_logger(), "%s: onFailure with error: %s", name().c_str(), toStr(error) ); - RCLCPP_INFO(rclcpp::get_logger("test"), " %s action pointer: %p", name().c_str(), reinterpret_cast(action_client_.get())); return NodeStatus::FAILURE; } void SleepAction::onHalt() { RCLCPP_INFO( node_->get_logger(), "%s: onHalt", name().c_str() ); - RCLCPP_INFO(rclcpp::get_logger("test"), " %s action pointer: %p", name().c_str(), reinterpret_cast(action_client_.get())); } // Plugin registration. diff --git a/btcpp_ros2_samples/src/sleep_client_shared.cpp b/btcpp_ros2_samples/src/sleep_client_shared.cpp index 12acd0b..e1793e2 100644 --- a/btcpp_ros2_samples/src/sleep_client_shared.cpp +++ b/btcpp_ros2_samples/src/sleep_client_shared.cpp @@ -60,14 +60,7 @@ class PrintValue : public BT::SyncActionNode )"; - // - // - // - // - // - // - // - // + int main(int argc, char **argv) { rclcpp::init(argc, argv); diff --git a/btcpp_ros2_samples/src/subscriber_shared_test.cpp b/btcpp_ros2_samples/src/subscriber_shared_test.cpp deleted file mode 100644 index 0fc0c34..0000000 --- a/btcpp_ros2_samples/src/subscriber_shared_test.cpp +++ /dev/null @@ -1,64 +0,0 @@ -#include "behaviortree_ros2/bt_topic_sub_shared_node.hpp" -#include - -using namespace BT; - -class ReceiveString: public RosTopicSubSharedNode -{ -public: - ReceiveString(const std::string& name, - const NodeConfig& conf, - const RosNodeParams& params) - : RosTopicSubSharedNode(name, conf, params) - {} - - static BT::PortsList providedPorts() - { - return {}; - } - - NodeStatus onTick(const std::shared_ptr& last_msg) override - { - RCLCPP_INFO(rclcpp::get_logger("test"), "subscriber pointer: %p", reinterpret_cast(sub_instance_->subscriber.get())); - if(last_msg) // empty if no new message received, since the last tick - { - RCLCPP_INFO(logger(), "[%s] new message: %s", name().c_str(), last_msg->data.c_str()); - } - return NodeStatus::SUCCESS; - } -}; - - // 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("subscriber_shared_test"); - - BehaviorTreeFactory factory; - - RosNodeParams params; - params.nh = nh; - params.default_port_value = "btcpp_string"; - factory.registerNodeType("ReceiveString", params); - - auto tree = factory.createTreeFromText(xml_text); - - while(rclcpp::ok()) - { - tree.tickWhileRunning(); - } - - return 0; -} diff --git a/btcpp_ros2_samples/src/subscriber_test.cpp b/btcpp_ros2_samples/src/subscriber_test.cpp index 132c596..195f1b9 100644 --- a/btcpp_ros2_samples/src/subscriber_test.cpp +++ b/btcpp_ros2_samples/src/subscriber_test.cpp @@ -19,7 +19,6 @@ class ReceiveString: public RosTopicSubNode NodeStatus onTick(const std::shared_ptr& last_msg) override { - RCLCPP_INFO(rclcpp::get_logger("test"), "subscriber pointer: %p", reinterpret_cast(sub_instance_->subscriber.get())); if(last_msg) // empty if no new message received, since the last tick { RCLCPP_INFO(logger(), "[%s] new message: %s", name().c_str(), last_msg->data.c_str()); From 34b0e93d99e1db15f54eb8212954b1cd763b0ef4 Mon Sep 17 00:00:00 2001 From: Rebecca Williams Date: Wed, 31 Jan 2024 10:02:11 +0000 Subject: [PATCH 11/18] add in subscriber shared node --- .../behaviortree_ros2/bt_action_node.hpp | 3 +- .../behaviortree_ros2/bt_service_node.hpp | 4 +- .../behaviortree_ros2/bt_topic_pub_node.hpp | 2 +- .../bt_topic_sub_shared_node.hpp | 324 ++++++++++++++++++ btcpp_ros2_samples/CMakeLists.txt | 4 + btcpp_ros2_samples/src/publisher_test.cpp | 3 +- .../src/subscriber_shared_test.cpp | 71 ++++ btcpp_ros2_samples/src/subscriber_test.cpp | 13 +- 8 files changed, 413 insertions(+), 11 deletions(-) create mode 100644 behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_shared_node.hpp create mode 100644 btcpp_ros2_samples/src/subscriber_shared_test.cpp diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp index 241bb37..ec64538 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp @@ -148,9 +148,10 @@ class RosActionNode : public BT::ActionNodeBase bool action_name_may_change_ = false; const std::chrono::milliseconds server_timeout_; const std::chrono::milliseconds wait_for_server_timeout_; - ActionClientPtr action_client_; + private: + ActionClientPtr action_client_; rclcpp::CallbackGroup::SharedPtr callback_group_; rclcpp::executors::SingleThreadedExecutor callback_group_executor_; diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp index 8cd6338..502503a 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp @@ -124,10 +124,10 @@ class RosServiceNode : public BT::ActionNodeBase bool service_name_may_change_ = false; const std::chrono::milliseconds service_timeout_; const std::chrono::milliseconds wait_for_service_timeout_; - typename std::shared_ptr service_client_; -private: +private: + typename std::shared_ptr service_client_; rclcpp::CallbackGroup::SharedPtr callback_group_; rclcpp::executors::SingleThreadedExecutor callback_group_executor_; diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_pub_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_pub_node.hpp index 00ea664..924bdb3 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_pub_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_pub_node.hpp @@ -92,10 +92,10 @@ class RosTopicPubNode : public BT::ConditionNode std::shared_ptr node_; std::string prev_topic_name_; bool topic_name_may_change_ = false; - std::shared_ptr publisher_; private: + std::shared_ptr publisher_; bool createPublisher(const std::string& topic_name); }; diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_shared_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_shared_node.hpp new file mode 100644 index 0000000..d375ab5 --- /dev/null +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_shared_node.hpp @@ -0,0 +1,324 @@ +// Copyright (c) 2023 Davide Faconti, Unmanned Life +// +// 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 +#include +#include +#include "behaviortree_cpp/condition_node.h" +#include "behaviortree_cpp/bt_factory.h" + +#include "behaviortree_ros2/ros_node_params.hpp" +#include + +namespace BT +{ + + +/** + * @brief Abstract class to wrap a Topic subscriber. + * Considering the example in the tutorial: + * https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Writing-A-Simple-Cpp-Publisher-And-Subscriber.html + * + * The corresponding wrapper would be: + * + * class SubscriberNode: RosTopicSubSharedNode + * + * The name of the topic will be determined as follows: + * + * 1. If a value is passes in the InputPort "topic_name", use that + * 2. Otherwise, use the value in RosNodeParams::default_port_value + */ +template +class RosTopicSubSharedNode : public BT::ConditionNode +{ + public: + // Type definitions + using Subscriber = typename rclcpp::Subscription; + + protected: + struct SubscriberInstance + { + void init(std::shared_ptr node, const std::string& topic_name) + { + // create a callback group for this particular instance + callback_group = + node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); + callback_group_executor.add_callback_group( + callback_group, node->get_node_base_interface()); + + rclcpp::SubscriptionOptions option; + option.callback_group = callback_group; + + // The callback will broadcast to all the instances of RosTopicSubSharedNode + auto callback = [this](const std::shared_ptr msg) + { + // RCLCPP_INFO(rclcpp::get_logger("test"), "new msg"); + broadcaster(msg); + }; + subscriber = node->create_subscription(topic_name, 1, callback, option); + } + + std::shared_ptr subscriber; + rclcpp::CallbackGroup::SharedPtr callback_group; + rclcpp::executors::SingleThreadedExecutor callback_group_executor; + boost::signals2::signal)> broadcaster; + + + }; + + static std::mutex& registryMutex() + { + static std::mutex sub_mutex; + return sub_mutex; + } + + // contains the fully-qualified name of the node and the name of the topic + static std::unordered_map>& getRegistry() + { + static std::unordered_map> subscribers_registry; + return subscribers_registry; + } + + std::shared_ptr node_; + std::shared_ptr sub_instance_ = nullptr; + std::shared_ptr last_msg_; + std::string topic_name_; + boost::signals2::connection signal_connection_; + std::string subscriber_key_; + static bool shared_resource_initialized; + static std::unordered_map> registry_; + + rclcpp::Logger logger() + { + return node_->get_logger(); + } + + public: + + /** 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 RosTopicSubSharedNode(const std::string & instance_name, + const BT::NodeConfig& conf, + const RosNodeParams& params); + + virtual ~RosTopicSubSharedNode() + { + signal_connection_.disconnect(); + // remove the subscribers from the static registry when the ALL the + // instances of RosTopicSubSharedNode are destroyed (i.e., the tree is destroyed) + if(sub_instance_) + { + sub_instance_.reset(); + std::unique_lock lk(registryMutex()); + auto& registry_ = getRegistry(); + auto it = registry_.find(subscriber_key_); + // when the reference count is 1, means that the only instance is owned by the + // registry itself. Delete it + if(it != registry_.end() && it->second.use_count() <= 1) + { + registry_.erase(it); + RCLCPP_INFO(logger(), "Remove subscriber [%s]", topic_name_.c_str() ); + } + } + } + + /** + * @brief Any subclass of RosTopicNode 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("topic_name", "__default__placeholder__", "Topic name") + }; + 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({}); + } + + NodeStatus tick() override final; + + /** Callback invoked in the tick. You must return either SUCCESS of FAILURE + * + * @param last_msg the latest message received, since the last tick. + * Will be empty if no new message received. + * + * @return the new status of the Node, based on last_msg + */ + virtual NodeStatus onTick(const std::shared_ptr& last_msg) = 0; + +private: + + bool createSubscriber(const std::string& topic_name); +}; + +//---------------------------------------------------------------- +//---------------------- DEFINITIONS ----------------------------- +//---------------------------------------------------------------- + +template inline + RosTopicSubSharedNode::RosTopicSubSharedNode(const std::string & instance_name, + const NodeConfig &conf, + const RosNodeParams& params) + : BT::ConditionNode(instance_name, conf), + node_(params.nh) +{ + // check port remapping + auto portIt = config().input_ports.find("topic_name"); + if(portIt != config().input_ports.end()) + { + const std::string& bb_topic_name = portIt->second; + + if(bb_topic_name.empty() || bb_topic_name == "__default__placeholder__") + { + if(params.default_port_value.empty()) { + throw std::logic_error( + "Both [topic_name] in the InputPort and the RosNodeParams are empty."); + } + else { + createSubscriber(params.default_port_value); + } + } + else if(!isBlackboardPointer(bb_topic_name)) + { + // If the content of the port "topic_name" is not + // a pointer to the blackboard, but a static string, we can + // create the client in the constructor. + createSubscriber(bb_topic_name); + } + else { + // do nothing + // createSubscriber will be invoked in the first tick(). + } + } + else { + if(params.default_port_value.empty()) { + throw std::logic_error( + "Both [topic_name] in the InputPort and the RosNodeParams are empty."); + } + else { + createSubscriber(params.default_port_value); + } + } +} + +template inline + bool RosTopicSubSharedNode::createSubscriber(const std::string& topic_name) +{ + if(topic_name.empty()) + { + throw RuntimeError("topic_name is empty"); + } + if(sub_instance_) + { + throw RuntimeError("Can't call createSubscriber more than once"); + } + + // find SubscriberInstance in the registry + std::unique_lock lk(registryMutex()); + subscriber_key_ = std::string(node_->get_fully_qualified_name()) + "/" + topic_name; + + auto& registry_ = getRegistry(); + auto it = registry_.find(subscriber_key_); + + if(!shared_resource_initialized && it == registry_.end()) + { + it = registry_.insert( {subscriber_key_, std::make_shared() }).first; + sub_instance_ = it->second; + sub_instance_->init(node_, topic_name); + + RCLCPP_INFO(logger(), + "Node [%s] created Subscriber to topic [%s]", + name().c_str(), topic_name.c_str() ); + + shared_resource_initialized = true; + } + else if(it == registry_.end()) + { + throw RuntimeError("Class shared resource has been initialized but is not in the registry!"); + } + else + { + sub_instance_ = it->second; + RCLCPP_INFO(logger(), + "Node [%s] retrieved Subscriber to topic [%s]", + name().c_str(), topic_name.c_str() ); + } + + // add "this" as received of the broadcaster + signal_connection_ = sub_instance_->broadcaster.connect( + [this](const std::shared_ptr msg) { last_msg_ = msg; } ); + + topic_name_ = topic_name; + return true; +} + + +template inline + NodeStatus RosTopicSubSharedNode::tick() +{ + // First, check if the subscriber_ is valid and that the name of the + // topic_name in the port didn't change. + // otherwise, create a new subscriber + if(!sub_instance_) + { + std::string topic_name; + getInput("topic_name", topic_name); + createSubscriber(topic_name); + } + + auto CheckStatus = [](NodeStatus status) + { + if( !isStatusCompleted(status) ) + { + throw std::logic_error("RosTopicSubSharedNode: the callback must return" + "either SUCCESS or FAILURE"); + } + return status; + }; + + // RCLCPP_INFO(rclcpp::get_logger("test"), "Tick, before spinsome"); + sub_instance_->callback_group_executor.spin_some(); + auto status = CheckStatus (onTick(last_msg_)); + last_msg_ = nullptr; + + return status; +} + +template +bool RosTopicSubSharedNode::shared_resource_initialized = false; + +// template +// typename std::shared_ptr> RosTopicPubSharedNode::publisher_ = nullptr; + +} // namespace BT + diff --git a/btcpp_ros2_samples/CMakeLists.txt b/btcpp_ros2_samples/CMakeLists.txt index c1be735..e440cab 100644 --- a/btcpp_ros2_samples/CMakeLists.txt +++ b/btcpp_ros2_samples/CMakeLists.txt @@ -51,6 +51,9 @@ ament_target_dependencies(sleep_server ${THIS_PACKAGE_DEPS}) add_executable(subscriber_test src/subscriber_test.cpp) ament_target_dependencies(subscriber_test ${THIS_PACKAGE_DEPS}) +add_executable(subscriber_shared_test src/subscriber_shared_test.cpp) +ament_target_dependencies(subscriber_shared_test ${THIS_PACKAGE_DEPS}) + ###################################################### # Build publisher_tests add_executable(publisher_test src/publisher_test.cpp) @@ -80,6 +83,7 @@ install(TARGETS sleep_server sleep_plugin subscriber_test + subscriber_shared_test publisher_test publisher_shared_test add_two_ints_server diff --git a/btcpp_ros2_samples/src/publisher_test.cpp b/btcpp_ros2_samples/src/publisher_test.cpp index 89a4b10..e8aa66e 100644 --- a/btcpp_ros2_samples/src/publisher_test.cpp +++ b/btcpp_ros2_samples/src/publisher_test.cpp @@ -27,7 +27,6 @@ class SendString: public RosTopicPubNode bool setMessage(std_msgs::msg::String& msg) { - // RCLCPP_INFO(rclcpp::get_logger("test"), "publisher pointer: %p", reinterpret_cast(publisher_.get())); std::string user_message = ""; getInput("message", user_message); msg.data = user_message; @@ -41,7 +40,7 @@ class SendString: public RosTopicPubNode - + diff --git a/btcpp_ros2_samples/src/subscriber_shared_test.cpp b/btcpp_ros2_samples/src/subscriber_shared_test.cpp new file mode 100644 index 0000000..8f9c6de --- /dev/null +++ b/btcpp_ros2_samples/src/subscriber_shared_test.cpp @@ -0,0 +1,71 @@ +#include "behaviortree_ros2/bt_topic_sub_shared_node.hpp" +#include + +using namespace BT; + +class ReceiveString: public RosTopicSubSharedNode +{ +public: + ReceiveString(const std::string& name, + const NodeConfig& conf, + const RosNodeParams& params) + : RosTopicSubSharedNode(name, conf, params) + {} + + static BT::PortsList providedPorts() + { + BT::PortsList addition; + BT::PortsList basic = {}; + basic.insert(addition.begin(), addition.end()); + + return providedBasicPorts(basic); + } + + NodeStatus onTick(const std::shared_ptr& last_msg) override + { + // RCLCPP_INFO(rclcpp::get_logger("test"), "%s, subscriber pointer: %p", name().c_str(), reinterpret_cast(sub_instance_->subscriber.get())); + if(last_msg) // empty if no new message received, since the last tick + { + RCLCPP_INFO(logger(), "[%s] new message: %s", name().c_str(), last_msg->data.c_str()); + } + return NodeStatus::SUCCESS; + } +}; + + // 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("subscriber_shared_test"); + + BehaviorTreeFactory factory; + + RosNodeParams params; + params.nh = nh; + params.default_port_value = "btcpp_string"; + factory.registerNodeType("ReceiveString", params); + + auto tree = factory.createTreeFromText(xml_text); + + while(rclcpp::ok()) + { + tree.tickWhileRunning(); + } + + return 0; +} diff --git a/btcpp_ros2_samples/src/subscriber_test.cpp b/btcpp_ros2_samples/src/subscriber_test.cpp index 195f1b9..1eb208c 100644 --- a/btcpp_ros2_samples/src/subscriber_test.cpp +++ b/btcpp_ros2_samples/src/subscriber_test.cpp @@ -12,9 +12,13 @@ class ReceiveString: public RosTopicSubNode : RosTopicSubNode(name, conf, params) {} - static BT::PortsList providedPorts() + static BT::PortsList providedPorts() { - return {}; + BT::PortsList addition; + BT::PortsList basic = {}; + basic.insert(addition.begin(), addition.end()); + + return providedBasicPorts(basic); } NodeStatus onTick(const std::shared_ptr& last_msg) override @@ -32,9 +36,8 @@ class ReceiveString: public RosTopicSubNode - - - + + From ab075cb948443b16c643c8b8c8e9eef9e5f74a98 Mon Sep 17 00:00:00 2001 From: Rebecca Williams Date: Wed, 31 Jan 2024 11:06:37 +0000 Subject: [PATCH 12/18] modify the bt_pub_node to work like the sub_node. --- .../behaviortree_ros2/bt_topic_pub_node.hpp | 74 +++++++++++++++++-- 1 file changed, 66 insertions(+), 8 deletions(-) diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_pub_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_pub_node.hpp index 924bdb3..8694fbf 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_pub_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_pub_node.hpp @@ -48,7 +48,19 @@ class RosTopicPubNode : public BT::ConditionNode const BT::NodeConfig& conf, const RosNodeParams& params); - virtual ~RosTopicPubNode() = default; + virtual ~RosTopicPubNode() + { + if (pub_instance_) { + pub_instance_.reset(); + std::unique_lock lk(registryMutex()); + auto& registry = getRegistry(); + auto it = registry.find(publisher_key_); + if (it != registry.end() && it->second.use_count() <= 1){ + registry.erase(it); + RCLCPP_INFO(logger(), "Remove publisher [%s]", topic_name_.c_str()); + } + } + } /** * @brief Any subclass of RosTopicPubNode that has additinal ports must provide a @@ -88,14 +100,41 @@ class RosTopicPubNode : public BT::ConditionNode virtual bool setMessage(TopicT& msg) = 0; protected: + struct PublisherInstance + { + void init(std::shared_ptr node, const std::string& topic_name) + { + publisher_ = node->create_publisher(topic_name, 1); + } + std::shared_ptr publisher_; + }; + + static std::mutex& registryMutex() + { + static std::mutex pub_mutex; + return pub_mutex; + } + + // contains the fully-qualified name of the node and the name of the topic + static std::unordered_map>& getRegistry() + { + static std::unordered_map> publishers_registry; + return publishers_registry; + } std::shared_ptr node_; - std::string prev_topic_name_; + std::string topic_name_; bool topic_name_may_change_ = false; + std::string publisher_key_; + std::shared_ptr pub_instance_ = nullptr; + rclcpp::Logger logger() + { + return node_->get_logger(); + } private: - std::shared_ptr publisher_; + bool createPublisher(const std::string& topic_name); }; @@ -157,8 +196,26 @@ template inline throw RuntimeError("topic_name is empty"); } - publisher_ = node_->create_publisher(topic_name, 1); - prev_topic_name_ = topic_name; + std::unique_lock lk(registryMutex()); + publisher_key_ = std::string(node_->get_fully_qualified_name()) + "/" + topic_name; + auto& registry = getRegistry(); + auto it = registry.find(publisher_key_); + if (it == registry.end()) + { + it = registry.insert( {publisher_key_, std::make_shared() }).first; + pub_instance_ = it->second; + pub_instance_->init(node_, topic_name); + + RCLCPP_INFO(logger(), + "Node [%s] created publisher topic [%s]", + name().c_str(), topic_name.c_str()); + } + else + { + pub_instance_ = it->second; + } + + topic_name_ = topic_name; return true; } @@ -172,11 +229,11 @@ template inline // First, check if the subscriber_ is valid and that the name of the // topic_name in the port didn't change. // otherwise, create a new subscriber - if(!publisher_ || (status() == NodeStatus::IDLE && topic_name_may_change_)) + if(!pub_instance_ || (status() == NodeStatus::IDLE && topic_name_may_change_)) { std::string topic_name; getInput("topic_name", topic_name); - if(prev_topic_name_ != topic_name) + if(topic_name_ != topic_name) { createPublisher(topic_name); } @@ -187,7 +244,8 @@ template inline { return NodeStatus::FAILURE; } - publisher_->publish(msg); + pub_instance_->publisher_->publish(msg); + // publisher_->publish(msg); return NodeStatus::SUCCESS; } From ea53e31bb48e3600d9f534d3f441f76aa6ea62f6 Mon Sep 17 00:00:00 2001 From: Rebecca Williams Date: Wed, 31 Jan 2024 11:40:42 +0000 Subject: [PATCH 13/18] modify the bt_service_node to work like the sub_node --- .../behaviortree_ros2/bt_service_node.hpp | 102 ++++++++++++++---- 1 file changed, 82 insertions(+), 20 deletions(-) diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp index 502503a..de2eaf6 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp @@ -62,7 +62,20 @@ class RosServiceNode : public BT::ActionNodeBase const BT::NodeConfig& conf, const BT::RosNodeParams& params); - virtual ~RosServiceNode() = default; + virtual ~RosServiceNode() + { + if (srv_instance_) + { + srv_instance_.reset(); + std::unique_lock lk(registryMutex()); + auto& registry = getRegistry(); + auto it = registry.find(service_clientkey_); + if (it != registry.end() && it->second.use_count() <= 1){ + registry.erase(it); + RCLCPP_INFO(logger(), "Remove service [%s]", service_name_.c_str()); + } + } + } /** * @brief Any subclass of RosServiceNode that has ports must implement a @@ -118,19 +131,51 @@ class RosServiceNode : public BT::ActionNodeBase } protected: + struct ServiceClientInstance + { + void init(std::shared_ptr node, const std::string& service_name) + { + callback_group = + node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); + callback_group_executor.add_callback_group( + callback_group, node->get_node_base_interface()); + + service_client = + node->create_client( + service_name, rmw_qos_profile_services_default, callback_group); + } + + std::shared_ptr service_client; + rclcpp::CallbackGroup::SharedPtr callback_group; + rclcpp::executors::SingleThreadedExecutor callback_group_executor; + }; + + static std::mutex& registryMutex() + { + static std::mutex sub_mutex; + return sub_mutex; + } + + // contains the fully-qualified name of the node and the name of the topic + static std::unordered_map>& getRegistry() + { + static std::unordered_map> service_clients_registry; + return service_clients_registry; + } std::shared_ptr node_; - std::string prev_service_name_; + std::shared_ptr srv_instance_ = nullptr; + std::string service_name_; bool service_name_may_change_ = false; const std::chrono::milliseconds service_timeout_; const std::chrono::milliseconds wait_for_service_timeout_; + std::string service_clientkey_; + rclcpp::Logger logger() + { + return node_->get_logger(); + } private: - - typename std::shared_ptr service_client_; - rclcpp::CallbackGroup::SharedPtr callback_group_; - rclcpp::executors::SingleThreadedExecutor callback_group_executor_; - std::shared_future future_response_; rclcpp::Time time_request_sent_; @@ -138,6 +183,7 @@ class RosServiceNode : public BT::ActionNodeBase bool response_received_; typename Response::SharedPtr response_; + bool createClient(const std::string &service_name); }; @@ -197,21 +243,37 @@ template inline template inline bool RosServiceNode::createClient(const std::string& service_name) { + RCLCPP_INFO(logger(), "Create service client function"); if(service_name.empty()) { throw RuntimeError("service_name is empty"); } - callback_group_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); - service_client_ = node_->create_client(service_name, rmw_qos_profile_services_default, callback_group_); - prev_service_name_ = service_name; + std::unique_lock lk(registryMutex()); + service_clientkey_ = std::string(node_->get_fully_qualified_name()) + "/" + service_name; + + auto& registry = getRegistry(); + auto it = registry.find(service_clientkey_); + if (it == registry.end()) + { + it = registry.insert({service_clientkey_, std::make_shared()}).first; + srv_instance_ = it->second; + srv_instance_->init(node_, service_name); + + RCLCPP_INFO(logger(), + "Node [%s] created service client [%s]", + name().c_str(), service_name.c_str() ); + } else { + srv_instance_ = it->second; + } + + service_name_ = service_name; - bool found = service_client_->wait_for_service(wait_for_service_timeout_); + bool found = srv_instance_->service_client->wait_for_service(wait_for_service_timeout_); if(!found) { - RCLCPP_ERROR(node_->get_logger(), "%s: Service with name '%s' is not reachable.", - name().c_str(), prev_service_name_.c_str()); + RCLCPP_ERROR(logger(), "%s: Service with name '%s' is not reachable.", + name().c_str(), service_name_.c_str()); } return found; } @@ -224,14 +286,14 @@ template inline halt(); return NodeStatus::FAILURE; } - // First, check if the service_client_ is valid and that the name of the + // First, check if the service_client is valid and that the name of the // service_name in the port didn't change. // otherwise, create a new client - if(!service_client_ || (status() == NodeStatus::IDLE && service_name_may_change_)) + if(!srv_instance_ || (status() == NodeStatus::IDLE && service_name_may_change_)) { std::string service_name; getInput("service_name", service_name); - if(prev_service_name_ != service_name) + if(service_name_ != service_name) { createClient(service_name); } @@ -263,7 +325,7 @@ template inline return CheckStatus( onFailure(INVALID_REQUEST) ); } - future_response_ = service_client_->async_send_request(request).share(); + future_response_ = srv_instance_->service_client->async_send_request(request).share(); time_request_sent_ = node_->now(); return NodeStatus::RUNNING; @@ -271,7 +333,7 @@ template inline if (status() == NodeStatus::RUNNING) { - callback_group_executor_.spin_some(); + srv_instance_->callback_group_executor.spin_some(); // FIRST case: check if the goal request has a timeout if( !response_received_ ) @@ -279,7 +341,7 @@ template inline auto const nodelay = std::chrono::milliseconds(0); auto const timeout = rclcpp::Duration::from_seconds( double(service_timeout_.count()) / 1000); - auto ret = callback_group_executor_.spin_until_future_complete(future_response_, nodelay); + auto ret = srv_instance_->callback_group_executor.spin_until_future_complete(future_response_, nodelay); if (ret != rclcpp::FutureReturnCode::SUCCESS) { From acd08d6da16c4c41b0f9e534ac460239cebb5492 Mon Sep 17 00:00:00 2001 From: Rebecca Williams Date: Wed, 31 Jan 2024 14:58:30 +0000 Subject: [PATCH 14/18] modify the bt_action_node to work like the sub_node --- .../behaviortree_ros2/bt_action_node.hpp | 112 +++++++++++++----- 1 file changed, 84 insertions(+), 28 deletions(-) diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp index ec64538..953cc01 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp @@ -69,7 +69,19 @@ class RosActionNode : public BT::ActionNodeBase const BT::NodeConfig& conf, const RosNodeParams& params); - virtual ~RosActionNode() = default; + virtual ~RosActionNode() + { + if (action_client_instance_) { + action_client_instance_.reset(); + std::unique_lock lk(registryMutex()); + auto& registry = getRegistry(); + auto it = registry.find(action_client_key_); + if (it != registry.end() && it->second.use_count() <= 1){ + registry.erase(it); + RCLCPP_INFO(logger(), "Remove action client [%s]", action_name_.c_str()); + } + } + } /** * @brief Any subclass of RosActionNode that has ports must implement a @@ -142,19 +154,54 @@ class RosActionNode : public BT::ActionNodeBase NodeStatus tick() override final; protected: + struct ActionClientInstance + { + void init(std::shared_ptr node, const std::string& action_name) + { + callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + callback_group_executor.add_callback_group(callback_group, node->get_node_base_interface()); + action_client = rclcpp_action::create_client(node, action_name, callback_group); + } + + ActionClientPtr action_client; + rclcpp::CallbackGroup::SharedPtr callback_group; + rclcpp::executors::SingleThreadedExecutor callback_group_executor; + typename ActionClient::SendGoalOptions goal_options; + }; + + static std::mutex& registryMutex() + { + static std::mutex sub_mutex; + return sub_mutex; + } + + static std::mutex& actionClientMutex() + { + static std::mutex action_client_mutex; + return action_client_mutex; + } + + rclcpp::Logger logger() + { + return node_->get_logger(); + } + + // contains the fully-qualified name of the node and the name of the topic + static std::unordered_map>& getRegistry() + { + static std::unordered_map> action_clients_registry; + return action_clients_registry; + } std::shared_ptr node_; - std::string prev_action_name_; + std::shared_ptr action_client_instance_; + std::string action_name_; bool action_name_may_change_ = false; const std::chrono::milliseconds server_timeout_; const std::chrono::milliseconds wait_for_server_timeout_; + std::string action_client_key_; private: - - ActionClientPtr action_client_; - rclcpp::CallbackGroup::SharedPtr callback_group_; - rclcpp::executors::SingleThreadedExecutor callback_group_executor_; - std::shared_future future_goal_handle_; typename GoalHandle::SharedPtr goal_handle_; @@ -232,17 +279,28 @@ template inline throw RuntimeError("action_name is empty"); } - callback_group_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); - action_client_ = rclcpp_action::create_client(node_, action_name, callback_group_); + std::unique_lock lk(registryMutex()); + action_client_key_ = std::string(node_->get_fully_qualified_name()) + "/" + action_name; + + auto& registry = getRegistry(); + auto it = registry.find(action_client_key_); + if(it == registry.end()) + { + it = registry.insert( {action_client_key_, std::make_shared()} ).first; + action_client_instance_ = it->second; + action_client_instance_->init(node_, action_name); + } + else{ + action_client_instance_ = it->second; + } - prev_action_name_ = action_name; + action_name_ = action_name; - bool found = action_client_->wait_for_action_server(wait_for_server_timeout_); + bool found = action_client_instance_->action_client->wait_for_action_server(wait_for_server_timeout_); if(!found) { RCLCPP_ERROR(node_->get_logger(), "%s: Action server with name '%s' is not reachable.", - name().c_str(), prev_action_name_.c_str()); + name().c_str(), action_name_.c_str()); } return found; } @@ -259,11 +317,11 @@ template inline // First, check if the action_client_ is valid and that the name of the // action_name in the port didn't change. // otherwise, create a new client - if(!action_client_ || (status() == NodeStatus::IDLE && action_name_may_change_)) + if(!action_client_instance_ || (status() == NodeStatus::IDLE && action_name_may_change_)) { std::string action_name; getInput("action_name", action_name); - if(prev_action_name_ != action_name) + if(action_name_ != action_name) { createClient(action_name); } @@ -334,7 +392,8 @@ template inline }; //-------------------- - future_goal_handle_ = action_client_->async_send_goal( goal, goal_options ); + future_goal_handle_ = action_client_instance_->action_client->async_send_goal( goal, goal_options ); + time_goal_sent_ = node_->now(); return NodeStatus::RUNNING; @@ -342,7 +401,8 @@ template inline if (status() == NodeStatus::RUNNING) { - callback_group_executor_.spin_some(); + std::unique_lock lock(actionClientMutex()); + action_client_instance_->callback_group_executor.spin_some(); // FIRST case: check if the goal request has a timeout if( !goal_received_ ) @@ -350,7 +410,7 @@ template inline auto nodelay = std::chrono::milliseconds(0); auto timeout = rclcpp::Duration::from_seconds( double(server_timeout_.count()) / 1000); - auto ret = callback_group_executor_.spin_until_future_complete(future_goal_handle_, nodelay); + auto ret = action_client_instance_->callback_group_executor.spin_until_future_complete(future_goal_handle_, nodelay); if (ret != rclcpp::FutureReturnCode::SUCCESS) { if( (node_->now() - time_goal_sent_) > timeout ) @@ -411,26 +471,22 @@ template inline template inline void RosActionNode::cancelGoal() { - auto future_result = action_client_->async_get_result(goal_handle_); - auto future_cancel = action_client_->async_cancel_goal(goal_handle_); + auto future_result = action_client_instance_->action_client->async_get_result(goal_handle_); + auto future_cancel = action_client_instance_->action_client->async_cancel_goal(goal_handle_); - if (callback_group_executor_.spin_until_future_complete(future_cancel, server_timeout_) != + if (action_client_instance_->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]", - prev_action_name_.c_str()); + action_name_.c_str()); } - if (callback_group_executor_.spin_until_future_complete(future_result, server_timeout_) != + if (action_client_instance_->callback_group_executor.spin_until_future_complete(future_result, server_timeout_) != rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR( node_->get_logger(), "Failed to get result call failed :( for [%s]", - prev_action_name_.c_str()); + action_name_.c_str()); } } - - - - } // namespace BT From 54d87046e3c534421dc84148d6e5bed00b36aaad Mon Sep 17 00:00:00 2001 From: Rebecca Williams Date: Wed, 31 Jan 2024 15:00:58 +0000 Subject: [PATCH 15/18] revert error codes and remove previous implementations --- .../bt_action_error_code.hpp | 31 -- .../behaviortree_ros2/bt_action_node.hpp | 24 + .../bt_action_shared_node.hpp | 493 ------------------ .../bt_service_error_code.hpp | 27 - .../behaviortree_ros2/bt_service_node.hpp | 21 +- .../bt_service_shared_node.hpp | 344 ------------ .../bt_topic_pub_shared_node.hpp | 203 -------- .../bt_topic_sub_shared_node.hpp | 324 ------------ 8 files changed, 44 insertions(+), 1423 deletions(-) delete mode 100644 behaviortree_ros2/include/behaviortree_ros2/bt_action_error_code.hpp delete mode 100644 behaviortree_ros2/include/behaviortree_ros2/bt_action_shared_node.hpp delete mode 100644 behaviortree_ros2/include/behaviortree_ros2/bt_service_error_code.hpp delete mode 100644 behaviortree_ros2/include/behaviortree_ros2/bt_service_shared_node.hpp delete mode 100644 behaviortree_ros2/include/behaviortree_ros2/bt_topic_pub_shared_node.hpp delete mode 100644 behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_shared_node.hpp diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_action_error_code.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_action_error_code.hpp deleted file mode 100644 index 0d4ca74..0000000 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_action_error_code.hpp +++ /dev/null @@ -1,31 +0,0 @@ -#ifndef BEHAVIORTREE_ROS2__BT_ACTION_ERROR_CODE_HPP_ -#define BEHAVIORTREE_ROS2__BT_ACTION_ERROR_CODE_HPP_ - -namespace BT -{ -enum ActionNodeErrorCode -{ - SERVER_UNREACHABLE, - SEND_GOAL_TIMEOUT, - GOAL_REJECTED_BY_SERVER, - ACTION_ABORTED, - ACTION_CANCELLED, - INVALID_GOAL -}; - -inline const char* toStr(const ActionNodeErrorCode& err) -{ - switch (err) - { - case SERVER_UNREACHABLE: return "SERVER_UNREACHABLE"; - case SEND_GOAL_TIMEOUT: return "SEND_GOAL_TIMEOUT"; - case GOAL_REJECTED_BY_SERVER: return "GOAL_REJECTED_BY_SERVER"; - case ACTION_ABORTED: return "ACTION_ABORTED"; - case ACTION_CANCELLED: return "ACTION_CANCELLED"; - case INVALID_GOAL: return "INVALID_GOAL"; - } - return nullptr; -} -} // namespace BT - -#endif // BEHAVIORTREE_ROS2__BT_ACTION_ERROR_CODE_HPP_ diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp index 953cc01..7640a79 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp @@ -28,6 +28,30 @@ namespace BT { +enum ActionNodeErrorCode +{ + SERVER_UNREACHABLE, + SEND_GOAL_TIMEOUT, + GOAL_REJECTED_BY_SERVER, + ACTION_ABORTED, + ACTION_CANCELLED, + INVALID_GOAL +}; + +inline const char* toStr(const ActionNodeErrorCode& err) +{ + switch (err) + { + case SERVER_UNREACHABLE: return "SERVER_UNREACHABLE"; + case SEND_GOAL_TIMEOUT: return "SEND_GOAL_TIMEOUT"; + case GOAL_REJECTED_BY_SERVER: return "GOAL_REJECTED_BY_SERVER"; + case ACTION_ABORTED: return "ACTION_ABORTED"; + case ACTION_CANCELLED: return "ACTION_CANCELLED"; + case INVALID_GOAL: return "INVALID_GOAL"; + } + return nullptr; +} + /** * @brief Abstract class to wrap rclcpp_action::Client<> * diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_action_shared_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_action_shared_node.hpp deleted file mode 100644 index 5b1a041..0000000 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_action_shared_node.hpp +++ /dev/null @@ -1,493 +0,0 @@ -// Copyright (c) 2018 Intel Corporation -// Copyright (c) 2023 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. - -#pragma once - -#include -#include -#include -#include -#include -#include "behaviortree_cpp/action_node.h" -#include "behaviortree_cpp/bt_factory.h" -#include "rclcpp_action/rclcpp_action.hpp" -#include "behaviortree_ros2/bt_action_error_code.hpp" - -#include "behaviortree_ros2/ros_node_params.hpp" - -namespace BT -{ -/** - * @brief Abstract class to wrap rclcpp_action::Client<> - * - * For instance, given the type AddTwoInts described in this tutorial: - * https://docs.ros.org/en/humble/Tutorials/Intermediate/Writing-an-Action-Server-Client/Cpp.html - * - * the corresponding wrapper would be: - * - * class FibonacciNode: public RosActionSharedNode - * - * RosActionSharedNode will try to be non-blocking for the entire duration of the call. - * The derived class must reimplement the virtual methods as described below. - * - * The name of the action will be determined as follows: - * - * 1. If a value is passes in the InputPort "action_name", use that - * 2. Otherwise, use the value in RosNodeParams::default_port_value - */ -template -class RosActionSharedNode : public BT::ActionNodeBase -{ - -public: - // Type definitions - using ActionType = ActionT; - using ActionClient = typename rclcpp_action::Client; - using ActionClientPtr = std::shared_ptr; - using Goal = typename ActionT::Goal; - using GoalHandle = typename rclcpp_action::ClientGoalHandle; - using WrappedResult = typename rclcpp_action::ClientGoalHandle::WrappedResult; - using Feedback = typename ActionT::Feedback; - - /** To register this class into the factory, use: - * - * factory.registerNodeType<>(node_name, params); - * - */ - explicit RosActionSharedNode(const std::string & instance_name, - const BT::NodeConfig& conf, - const RosNodeParams& params); - - virtual ~RosActionSharedNode(); - - /** - * @brief Any subclass of RosActionSharedNode that has ports must implement 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("action_name", "__default__placeholder__", "Action server name") - }; - 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 Callback executed when the node is halted. Note that cancelGoal() - /// is done automatically. - virtual void onHalt() {} - - /** setGoal s a callback that allows the user to set - * the goal message (ActionT::Goal). - * - * @param goal the goal to be sent to the action server. - * - * @return false if the request should not be sent. In that case, - * RosActionSharedNode::onFailure(INVALID_GOAL) will be called. - */ - virtual bool setGoal(Goal& goal) = 0; - - /** 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; - - /** 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 onFeedback(const std::shared_ptr /*feedback*/) - { - return NodeStatus::RUNNING; - } - - /** Callback invoked when something goes wrong. - * It must return either SUCCESS or FAILURE. - */ - virtual BT::NodeStatus onFailure(ActionNodeErrorCode /*error*/) - { - return NodeStatus::FAILURE; - } - - /// Method used to send a request to the Action server to cancel the current goal - void cancelGoal(); - - - /// The default halt() implementation will call cancelGoal if necessary. - void halt() override final; - - NodeStatus tick() override final; - -protected: - - std::shared_ptr node_; - std::string current_action_name_; - bool action_name_may_change_ = false; - const std::chrono::milliseconds server_timeout_; - const std::chrono::milliseconds wait_for_server_timeout_; - static bool shared_resource_initialized; - static ActionClientPtr action_client_; - static std::mutex action_client_mutex_; - - -private: - - rclcpp::CallbackGroup::SharedPtr callback_group_; - static std::shared_ptr callback_group_executor_; - - std::shared_future future_goal_handle_; - typename GoalHandle::SharedPtr goal_handle_; - - rclcpp::Time time_goal_sent_; - NodeStatus on_feedback_state_change_; - bool goal_received_; - WrappedResult result_; - - bool createClient(const std::string &action_name); -}; - -//---------------------------------------------------------------- -//---------------------- DEFINITIONS ----------------------------- -//---------------------------------------------------------------- - -template inline - RosActionSharedNode::RosActionSharedNode(const std::string & instance_name, - const NodeConfig &conf, - const RosNodeParams ¶ms): - BT::ActionNodeBase(instance_name, conf), - node_(params.nh), - server_timeout_(params.server_timeout), - wait_for_server_timeout_(params.wait_for_server_timeout) -{ - // Three cases: - // - we use the default action_name in RosNodeParams when port is empty - // - we use the action_name in the port and it is a static string. - // - we use the action_name in the port and it is blackboard entry. - - // check port remapping - auto portIt = config().input_ports.find("action_name"); - if(portIt != config().input_ports.end()) - { - const std::string& bb_action_name = portIt->second; - - if(bb_action_name.empty() || bb_action_name == "__default__placeholder__") - { - if(params.default_port_value.empty()) { - throw std::logic_error( - "Both [action_name] in the InputPort and the RosNodeParams are empty."); - } - else { - createClient(params.default_port_value); - } - } - else if(!isBlackboardPointer(bb_action_name)) - { - // If the content of the port "action_name" is not - // a pointer to the blackboard, but a static string, we can - // create the client in the constructor. - createClient(bb_action_name); - } - else { - action_name_may_change_ = true; - // createClient will be invoked in the first tick(). - } - } - else { - - if(params.default_port_value.empty()) { - throw std::logic_error( - "Both [action_name] in the InputPort and the RosNodeParams are empty."); - } - else { - createClient(params.default_port_value); - } - } -} - -template -RosActionSharedNode::~RosActionSharedNode() -{ - RCLCPP_INFO(node_->get_logger(), "Destroying Action server for %s", current_action_name_.c_str()); - if (action_client_ && goal_handle_) { - auto state = goal_handle_->get_status(); - if (state == rclcpp_action::GoalStatus::STATUS_ACCEPTED || - state == rclcpp_action::GoalStatus::STATUS_EXECUTING) { - try { - auto future_cancel = action_client_->async_cancel_goal(goal_handle_); - if (callback_group_executor_) { - callback_group_executor_->spin_some(); - } - } catch (const std::exception& e) { - RCLCPP_ERROR(node_->get_logger(), "Exception during goal cancellation in destructor: %s", e.what()); - } - } - } - - - if (callback_group_executor_) { - callback_group_executor_.reset(); - } - - if (action_client_) { - action_client_.reset(); - } - - RCLCPP_INFO(node_->get_logger(), "Action server %s destroyed", current_action_name_.c_str()); -} - -template inline -bool RosActionSharedNode::createClient(const std::string& action_name) -{ - std::lock_guard lock(action_client_mutex_); - if (action_name.empty()) - { - throw RuntimeError("action_name is empty"); - } - - if (!shared_resource_initialized) - { - RCLCPP_INFO(node_->get_logger(), "Creating action client for %s", action_name.c_str()); - callback_group_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - if (callback_group_executor_ == nullptr) - { - callback_group_executor_ = std::make_shared(); - } - callback_group_executor_->add_callback_group(callback_group_, node_->get_node_base_interface()); - action_client_ = rclcpp_action::create_client(node_, action_name, callback_group_); - - current_action_name_ = action_name; - shared_resource_initialized = true; - } - - bool found = action_client_->wait_for_action_server(wait_for_server_timeout_); - if(!found) - { - RCLCPP_ERROR(node_->get_logger(), "%s: Action server with name '%s' is not reachable.", - name().c_str(), current_action_name_.c_str()); - } - return found; -} - -template inline - NodeStatus RosActionSharedNode::tick() -{ - if (!rclcpp::ok()) - { - halt(); - return NodeStatus::FAILURE; - } - - // First, check if the action_client_ is valid and that the name of the - // action_name in the port didn't change. - // otherwise, create a new client - if(!action_client_ || (status() == NodeStatus::IDLE && action_name_may_change_)) - { - RCLCPP_INFO(node_->get_logger(), "Client not initialized or action_name changed"); - std::string action_name; - getInput("action_name", action_name); - if(current_action_name_ != action_name) - { - createClient(action_name); - } - } - - //------------------------------------------ - auto CheckStatus = [](NodeStatus status) - { - if( !isStatusCompleted(status) ) - { - throw std::logic_error("RosActionSharedNode: the callback must return either SUCCESS of FAILURE"); - } - return status; - }; - - // first step to be done only at the beginning of the Action - if (status() == BT::NodeStatus::IDLE) - { - RCLCPP_INFO(node_->get_logger(), "Client IDLE sending goal"); - setStatus(NodeStatus::RUNNING); - - goal_received_ = false; - future_goal_handle_ = {}; - on_feedback_state_change_ = NodeStatus::RUNNING; - result_ = {}; - - Goal goal; - - if( !setGoal(goal) ) - { - return CheckStatus( onFailure(INVALID_GOAL) ); - } - - typename ActionClient::SendGoalOptions goal_options; - - //-------------------- - goal_options.feedback_callback = - [this](typename GoalHandle::SharedPtr, - const std::shared_ptr feedback) - { - on_feedback_state_change_ = onFeedback(feedback); - if( on_feedback_state_change_ == NodeStatus::IDLE) - { - throw std::logic_error("onFeedback must not return IDLE"); - } - emitWakeUpSignal(); - }; - //-------------------- - goal_options.result_callback = - [this](const WrappedResult& result) - { - if (goal_handle_->get_goal_id() == result.goal_id) { - RCLCPP_DEBUG( node_->get_logger(), "result_callback" ); - result_ = result; - emitWakeUpSignal(); - } - }; - //-------------------- - goal_options.goal_response_callback = - [this](typename GoalHandle::SharedPtr const future_handle) - { - auto goal_handle_ = future_handle.get(); - if (!goal_handle_) - { - RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server"); - } else { - RCLCPP_DEBUG(node_->get_logger(), "Goal accepted by server, waiting for result"); - } - }; - //-------------------- - RCLCPP_INFO(node_->get_logger(), "Goal sent to server"); - future_goal_handle_ = action_client_->async_send_goal( goal, goal_options ); - time_goal_sent_ = node_->now(); - - return NodeStatus::RUNNING; - } - - if (status() == NodeStatus::RUNNING) - { - std::lock_guard lock(action_client_mutex_); - callback_group_executor_->spin_some(); - // 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); - - auto ret = callback_group_executor_->spin_until_future_complete(future_goal_handle_, nodelay); - - if (ret != rclcpp::FutureReturnCode::SUCCESS) - { - if( (node_->now() - time_goal_sent_) > timeout ) - { - return CheckStatus( onFailure(SEND_GOAL_TIMEOUT) ); - } - else{ - return NodeStatus::RUNNING; - } - } - else - { - goal_received_ = true; - goal_handle_ = future_goal_handle_.get(); - future_goal_handle_ = {}; - - if (!goal_handle_) { - return CheckStatus( onFailure( GOAL_REJECTED_BY_SERVER ) ); - } - } - } - - // SECOND case: onFeedback requested a stop - if( on_feedback_state_change_ != NodeStatus::RUNNING ) - - { - cancelGoal(); - return on_feedback_state_change_; - } - // 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_ ) ); - } - } - } - return NodeStatus::RUNNING; -} - -template inline - void RosActionSharedNode::halt() -{ - if(status() == BT::NodeStatus::RUNNING) - { - cancelGoal(); - onHalt(); - } -} - -template inline - void RosActionSharedNode::cancelGoal() -{ - std::lock_guard lock(action_client_mutex_); - auto future_result = action_client_->async_get_result(goal_handle_); - 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]", - current_action_name_.c_str()); - } - - if (callback_group_executor_->spin_until_future_complete(future_result, server_timeout_) != - rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_ERROR( node_->get_logger(), "Failed to get result call failed :( for [%s]", - current_action_name_.c_str()); - } -} - -template -typename std::shared_ptr> RosActionSharedNode::action_client_ = nullptr; - -template -std::shared_ptr RosActionSharedNode::callback_group_executor_ = nullptr; - -template -bool RosActionSharedNode::shared_resource_initialized = false; - -template -std::mutex RosActionSharedNode::action_client_mutex_; - -} // namespace BT diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_service_error_code.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_service_error_code.hpp deleted file mode 100644 index 10c485a..0000000 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_service_error_code.hpp +++ /dev/null @@ -1,27 +0,0 @@ -#ifndef BEHAVIORTREE_ROS2__BT_SERVICE_ERROR_CODE_HPP_ -#define BEHAVIORTREE_ROS2__BT_SERVICE_ERROR_CODE_HPP_ - -namespace BT -{ -enum ServiceNodeErrorCode -{ - SERVICE_UNREACHABLE, - SERVICE_TIMEOUT, - INVALID_REQUEST, - SERVICE_ABORTED -}; - -inline const char* toStr(const ServiceNodeErrorCode& err) -{ - switch (err) - { - case SERVICE_UNREACHABLE: return "SERVICE_UNREACHABLE"; - case SERVICE_TIMEOUT: return "SERVICE_TIMEOUT"; - case INVALID_REQUEST: return "INVALID_REQUEST"; - case SERVICE_ABORTED: return "SERVICE_ABORTED"; - } - return nullptr; -} -} // namespace BT - -#endif // BEHAVIORTREE_ROS2__BT_SERVICE_ERROR_CODE_HPP_ diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp index de2eaf6..c405d98 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp @@ -22,10 +22,29 @@ #include "behaviortree_cpp/bt_factory.h" #include "behaviortree_ros2/ros_node_params.hpp" -#include "behaviortree_ros2/bt_service_error_code.hpp" namespace BT { +enum ServiceNodeErrorCode +{ + SERVICE_UNREACHABLE, + SERVICE_TIMEOUT, + INVALID_REQUEST, + SERVICE_ABORTED +}; + +inline const char* toStr(const ServiceNodeErrorCode& err) +{ + switch (err) + { + case SERVICE_UNREACHABLE: return "SERVICE_UNREACHABLE"; + case SERVICE_TIMEOUT: return "SERVICE_TIMEOUT"; + case INVALID_REQUEST: return "INVALID_REQUEST"; + case SERVICE_ABORTED: return "SERVICE_ABORTED"; + } + return nullptr; +} + /** * @brief Abstract class use to wrap rclcpp::Client<> * diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_service_shared_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_service_shared_node.hpp deleted file mode 100644 index 5127873..0000000 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_service_shared_node.hpp +++ /dev/null @@ -1,344 +0,0 @@ -// Copyright (c) 2019 Intel Corporation -// Copyright (c) 2023 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. - -#pragma once - -#include -#include -#include -#include -#include -#include "behaviortree_cpp/bt_factory.h" - -#include "behaviortree_ros2/ros_node_params.hpp" -#include "behaviortree_ros2/bt_service_error_code.hpp" - -namespace BT -{ -/** - * @brief Abstract class use to wrap rclcpp::Client<> - * - * For instance, given the type AddTwoInts described in this tutorial: - * https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Writing-A-Simple-Cpp-Service-And-Client.html - * - * the corresponding wrapper would be: - * - * class AddTwoNumbers: public RosServiceSharedNode - * - * RosServiceSharedNode will try to be non-blocking for the entire duration of the call. - * The derived class must reimplement the virtual methods as described below. - * - * The name of the service will be determined as follows: - * - * 1. If a value is passes in the InputPort "service_name", use that - * 2. Otherwise, use the value in RosNodeParams::default_port_value - */ -template -class RosServiceSharedNode : public BT::ActionNodeBase -{ - -public: - // Type definitions - using ServiceClient = typename rclcpp::Client; - using Request = typename ServiceT::Request; - using Response = typename ServiceT::Response; - - /** To register this class into the factory, use: - * - * factory.registerNodeType<>(node_name, params); - */ - explicit RosServiceSharedNode(const std::string & instance_name, - const BT::NodeConfig& conf, - const BT::RosNodeParams& params); - - virtual ~RosServiceSharedNode() = default; - - /** - * @brief Any subclass of RosServiceSharedNode that has ports must implement 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", "__default__placeholder__", "Service name") - }; - 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({}); - } - - NodeStatus tick() override final; - - /// The default halt() implementation. - void halt() override; - - /** setRequest is a callback that allows the user to set - * the request message (ServiceT::Request). - * - * @param request the request to be sent to the service provider. - * - * @return false if the request should not be sent. In that case, - * RosServiceSharedNode::onFailure(INVALID_REQUEST) will be called. - */ - virtual bool setRequest(typename Request::SharedPtr& request) = 0; - - /** Callback invoked when the response is received by the server. - * It is up to the user to define if this returns SUCCESS or FAILURE. - */ - virtual BT::NodeStatus onResponseReceived(const typename Response::SharedPtr& response) = 0; - - /** Callback invoked when something goes wrong; you can override it. - * It must return either SUCCESS or FAILURE. - */ - virtual BT::NodeStatus onFailure(ServiceNodeErrorCode /*error*/) - { - return NodeStatus::FAILURE; - } - -protected: - - std::shared_ptr node_; - std::string prev_service_name_; - bool service_name_may_change_ = false; - const std::chrono::milliseconds service_timeout_; - const std::chrono::milliseconds wait_for_service_timeout_; - static typename std::shared_ptr service_client_; - static bool shared_resource_initialized; -private: - rclcpp::CallbackGroup::SharedPtr callback_group_; - static std::shared_ptr callback_group_executor_; - static std::mutex mutex_; - - std::shared_future future_response_; - - rclcpp::Time time_request_sent_; - NodeStatus on_feedback_state_change_; - bool response_received_; - typename Response::SharedPtr response_; - - bool createClient(const std::string &service_name); -}; - -//---------------------------------------------------------------- -//---------------------- DEFINITIONS ----------------------------- -//---------------------------------------------------------------- - -template inline - RosServiceSharedNode::RosServiceSharedNode(const std::string & instance_name, - const NodeConfig &conf, - const RosNodeParams& params): - BT::ActionNodeBase(instance_name, conf), - node_(params.nh), - service_timeout_(params.server_timeout), - wait_for_service_timeout_(params.wait_for_server_timeout) -{ - // check port remapping - auto portIt = config().input_ports.find("service_name"); - if(portIt != config().input_ports.end()) - { - const std::string& bb_service_name = portIt->second; - - if(bb_service_name.empty() || bb_service_name == "__default__placeholder__") - { - if(params.default_port_value.empty()) { - throw std::logic_error( - "Both [service_name] in the InputPort and the RosNodeParams are empty."); - } - else { - createClient(params.default_port_value); - } - } - else if(!isBlackboardPointer(bb_service_name)) - { - // If the content of the port "service_name" is not - // a pointer to the blackboard, but a static string, we can - // create the client in the constructor. - createClient(bb_service_name); - } - else { - service_name_may_change_ = true; - // createClient will be invoked in the first tick(). - } - } - else { - - if(params.default_port_value.empty()) { - throw std::logic_error( - "Both [service_name] in the InputPort and the RosNodeParams are empty."); - } - else { - createClient(params.default_port_value); - } - } -} - -template inline - bool RosServiceSharedNode::createClient(const std::string& service_name) -{ - std::lock_guard lock(mutex_); - if(service_name.empty()) - { - throw RuntimeError("service_name is empty"); - } - - if (!shared_resource_initialized) { - callback_group_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - if (callback_group_executor_ == nullptr) { - callback_group_executor_ = std::make_shared(); - } - callback_group_executor_->add_callback_group(callback_group_, node_->get_node_base_interface()); - service_client_ = node_->create_client(service_name, rmw_qos_profile_services_default, callback_group_); - prev_service_name_ = service_name; - shared_resource_initialized = true; - } - - - bool found = service_client_->wait_for_service(wait_for_service_timeout_); - if(!found) - { - RCLCPP_ERROR(node_->get_logger(), "%s: Service with name '%s' is not reachable.", - name().c_str(), prev_service_name_.c_str()); - } - return found; -} - -template inline - NodeStatus RosServiceSharedNode::tick() -{ - if (!rclcpp::ok()) - { - halt(); - return NodeStatus::FAILURE; - } - - // First, check if the service_client_ is valid and that the name of the - // service_name in the port didn't change. - // otherwise, create a new client - if(!service_client_ || (status() == NodeStatus::IDLE && service_name_may_change_)) - { - std::string service_name; - getInput("service_name", service_name); - if(prev_service_name_ != service_name) - { - createClient(service_name); - } - } - - auto CheckStatus = [](NodeStatus status) - { - if( !isStatusCompleted(status) ) - { - throw std::logic_error("RosServiceSharedNode: the callback must return either SUCCESS or FAILURE"); - } - return status; - }; - - // first step to be done only at the beginning of the Action - if (status() == BT::NodeStatus::IDLE) - { - setStatus(NodeStatus::RUNNING); - - response_received_ = false; - future_response_ = {}; - on_feedback_state_change_ = NodeStatus::RUNNING; - response_ = {}; - - typename Request::SharedPtr request = std::make_shared(); - - if( !setRequest(request) ) - { - return CheckStatus( onFailure(INVALID_REQUEST) ); - } - - future_response_ = service_client_->async_send_request(request).share(); - time_request_sent_ = node_->now(); - - return NodeStatus::RUNNING; - } - - if (status() == NodeStatus::RUNNING) - { - std::lock_guard lock(mutex_); - callback_group_executor_->spin_some(); - - // FIRST case: check if the goal request has a timeout - if( !response_received_ ) - { - auto const nodelay = std::chrono::milliseconds(0); - auto const timeout = rclcpp::Duration::from_seconds( double(service_timeout_.count()) / 1000); - - auto ret = callback_group_executor_->spin_until_future_complete(future_response_, nodelay); - - if (ret != rclcpp::FutureReturnCode::SUCCESS) - { - if( (node_->now() - time_request_sent_) > timeout ) - { - return CheckStatus( onFailure(SERVICE_TIMEOUT) ); - } - else{ - return NodeStatus::RUNNING; - } - } - else - { - response_received_ = true; - response_ = future_response_.get(); - future_response_ = {}; - - if (!response_) { - throw std::runtime_error("Request was rejected by the service"); - } - } - } - - // SECOND case: response received - return CheckStatus( onResponseReceived( response_ ) ); - } - return NodeStatus::RUNNING; -} - -template inline - void RosServiceSharedNode::halt() -{ - if( status() == NodeStatus::RUNNING ) - { - resetStatus(); - } -} - -template -bool RosServiceSharedNode::shared_resource_initialized = false; - -template -typename std::shared_ptr> RosServiceSharedNode::service_client_ = nullptr; - -template -std::mutex RosServiceSharedNode::mutex_; - -template -std::shared_ptr RosServiceSharedNode::callback_group_executor_ = nullptr; -} // namespace BT - diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_pub_shared_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_pub_shared_node.hpp deleted file mode 100644 index c95f2aa..0000000 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_pub_shared_node.hpp +++ /dev/null @@ -1,203 +0,0 @@ -// Copyright (c) 2023 Davide Faconti, Unmanned Life -// -// 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 -#include -#include -#include "behaviortree_cpp/condition_node.h" -#include "behaviortree_cpp/bt_factory.h" -#include "behaviortree_ros2/ros_node_params.hpp" - -namespace BT -{ - -/** - * @brief Abstract class to wrap a ROS publisher - * - */ -template -class RosTopicPubSharedNode : public BT::ConditionNode -{ - -public: - // Type definitions - using Publisher = typename rclcpp::Publisher; - - /** 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 RosTopicPubSharedNode(const std::string & instance_name, - const BT::NodeConfig& conf, - const RosNodeParams& params); - - virtual ~RosTopicPubSharedNode() = default; - - /** - * @brief Any subclass of RosTopicPubSharedNode that has additinal ports 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("topic_name", "__default__placeholder__", "Topic name") - }; - 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({}); - } - - NodeStatus tick() override final; - - /** - * @brief setMessage is a callback invoked in tick to allow the user to pass - * the message to be published. - * - * @param msg the message. - * @return return false if anything is wrong and we must not send the message. - * the Condition will return FAILURE. - */ - virtual bool setMessage(TopicT& msg) = 0; - -protected: - - std::shared_ptr node_; - std::string prev_topic_name_; - bool topic_name_may_change_ = false; - static bool shared_resource_initialized; - static std::shared_ptr publisher_; -private: - bool createPublisher(const std::string& topic_name); -}; - -//---------------------------------------------------------------- -//---------------------- DEFINITIONS ----------------------------- -//---------------------------------------------------------------- - -template inline - RosTopicPubSharedNode::RosTopicPubSharedNode(const std::string & instance_name, - const NodeConfig &conf, - const RosNodeParams& params) - : BT::ConditionNode(instance_name, conf), - node_(params.nh) -{ - // check port remapping - auto portIt = config().input_ports.find("topic_name"); - if(portIt != config().input_ports.end()) - { - const std::string& bb_topic_name = portIt->second; - - if(bb_topic_name.empty() || bb_topic_name == "__default__placeholder__") - { - if(params.default_port_value.empty()) { - throw std::logic_error( - "Both [topic_name] in the InputPort and the RosNodeParams are empty."); - } - else { - createPublisher(params.default_port_value); - } - } - else if(!isBlackboardPointer(bb_topic_name)) - { - // If the content of the port "topic_name" is not - // a pointer to the blackboard, but a static string, we can - // create the client in the constructor. - createPublisher(bb_topic_name); - } - else { - topic_name_may_change_ = true; - // createPublisher will be invoked in the first tick(). - } - } - else { - if(params.default_port_value.empty()) { - throw std::logic_error( - "Both [topic_name] in the InputPort and the RosNodeParams are empty."); - } - else { - createPublisher(params.default_port_value); - } - } -} - -template inline - bool RosTopicPubSharedNode::createPublisher(const std::string& topic_name) -{ - if(topic_name.empty()) - { - throw RuntimeError("topic_name is empty"); - } - - if (!shared_resource_initialized) { - publisher_ = node_->create_publisher(topic_name, 1); - prev_topic_name_ = topic_name; - shared_resource_initialized = true; - } - - return true; -} - -template inline - NodeStatus RosTopicPubSharedNode::tick() -{ - if (!rclcpp::ok()) - { - return NodeStatus::FAILURE; - } - // First, check if the subscriber_ is valid and that the name of the - // topic_name in the port didn't change. - // otherwise, create a new subscriber - if(!publisher_ || (status() == NodeStatus::IDLE && topic_name_may_change_)) - { - std::string topic_name; - getInput("topic_name", topic_name); - if(prev_topic_name_ != topic_name) - { - createPublisher(topic_name); - } - } - - T msg; - if (!setMessage(msg)) - { - return NodeStatus::FAILURE; - } - publisher_->publish(msg); - return NodeStatus::SUCCESS; -} - -template -bool RosTopicPubSharedNode::shared_resource_initialized = false; - -template -typename std::shared_ptr> RosTopicPubSharedNode::publisher_ = nullptr; -} // namespace BT - diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_shared_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_shared_node.hpp deleted file mode 100644 index d375ab5..0000000 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_shared_node.hpp +++ /dev/null @@ -1,324 +0,0 @@ -// Copyright (c) 2023 Davide Faconti, Unmanned Life -// -// 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 -#include -#include -#include "behaviortree_cpp/condition_node.h" -#include "behaviortree_cpp/bt_factory.h" - -#include "behaviortree_ros2/ros_node_params.hpp" -#include - -namespace BT -{ - - -/** - * @brief Abstract class to wrap a Topic subscriber. - * Considering the example in the tutorial: - * https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Writing-A-Simple-Cpp-Publisher-And-Subscriber.html - * - * The corresponding wrapper would be: - * - * class SubscriberNode: RosTopicSubSharedNode - * - * The name of the topic will be determined as follows: - * - * 1. If a value is passes in the InputPort "topic_name", use that - * 2. Otherwise, use the value in RosNodeParams::default_port_value - */ -template -class RosTopicSubSharedNode : public BT::ConditionNode -{ - public: - // Type definitions - using Subscriber = typename rclcpp::Subscription; - - protected: - struct SubscriberInstance - { - void init(std::shared_ptr node, const std::string& topic_name) - { - // create a callback group for this particular instance - callback_group = - node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); - callback_group_executor.add_callback_group( - callback_group, node->get_node_base_interface()); - - rclcpp::SubscriptionOptions option; - option.callback_group = callback_group; - - // The callback will broadcast to all the instances of RosTopicSubSharedNode - auto callback = [this](const std::shared_ptr msg) - { - // RCLCPP_INFO(rclcpp::get_logger("test"), "new msg"); - broadcaster(msg); - }; - subscriber = node->create_subscription(topic_name, 1, callback, option); - } - - std::shared_ptr subscriber; - rclcpp::CallbackGroup::SharedPtr callback_group; - rclcpp::executors::SingleThreadedExecutor callback_group_executor; - boost::signals2::signal)> broadcaster; - - - }; - - static std::mutex& registryMutex() - { - static std::mutex sub_mutex; - return sub_mutex; - } - - // contains the fully-qualified name of the node and the name of the topic - static std::unordered_map>& getRegistry() - { - static std::unordered_map> subscribers_registry; - return subscribers_registry; - } - - std::shared_ptr node_; - std::shared_ptr sub_instance_ = nullptr; - std::shared_ptr last_msg_; - std::string topic_name_; - boost::signals2::connection signal_connection_; - std::string subscriber_key_; - static bool shared_resource_initialized; - static std::unordered_map> registry_; - - rclcpp::Logger logger() - { - return node_->get_logger(); - } - - public: - - /** 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 RosTopicSubSharedNode(const std::string & instance_name, - const BT::NodeConfig& conf, - const RosNodeParams& params); - - virtual ~RosTopicSubSharedNode() - { - signal_connection_.disconnect(); - // remove the subscribers from the static registry when the ALL the - // instances of RosTopicSubSharedNode are destroyed (i.e., the tree is destroyed) - if(sub_instance_) - { - sub_instance_.reset(); - std::unique_lock lk(registryMutex()); - auto& registry_ = getRegistry(); - auto it = registry_.find(subscriber_key_); - // when the reference count is 1, means that the only instance is owned by the - // registry itself. Delete it - if(it != registry_.end() && it->second.use_count() <= 1) - { - registry_.erase(it); - RCLCPP_INFO(logger(), "Remove subscriber [%s]", topic_name_.c_str() ); - } - } - } - - /** - * @brief Any subclass of RosTopicNode 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("topic_name", "__default__placeholder__", "Topic name") - }; - 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({}); - } - - NodeStatus tick() override final; - - /** Callback invoked in the tick. You must return either SUCCESS of FAILURE - * - * @param last_msg the latest message received, since the last tick. - * Will be empty if no new message received. - * - * @return the new status of the Node, based on last_msg - */ - virtual NodeStatus onTick(const std::shared_ptr& last_msg) = 0; - -private: - - bool createSubscriber(const std::string& topic_name); -}; - -//---------------------------------------------------------------- -//---------------------- DEFINITIONS ----------------------------- -//---------------------------------------------------------------- - -template inline - RosTopicSubSharedNode::RosTopicSubSharedNode(const std::string & instance_name, - const NodeConfig &conf, - const RosNodeParams& params) - : BT::ConditionNode(instance_name, conf), - node_(params.nh) -{ - // check port remapping - auto portIt = config().input_ports.find("topic_name"); - if(portIt != config().input_ports.end()) - { - const std::string& bb_topic_name = portIt->second; - - if(bb_topic_name.empty() || bb_topic_name == "__default__placeholder__") - { - if(params.default_port_value.empty()) { - throw std::logic_error( - "Both [topic_name] in the InputPort and the RosNodeParams are empty."); - } - else { - createSubscriber(params.default_port_value); - } - } - else if(!isBlackboardPointer(bb_topic_name)) - { - // If the content of the port "topic_name" is not - // a pointer to the blackboard, but a static string, we can - // create the client in the constructor. - createSubscriber(bb_topic_name); - } - else { - // do nothing - // createSubscriber will be invoked in the first tick(). - } - } - else { - if(params.default_port_value.empty()) { - throw std::logic_error( - "Both [topic_name] in the InputPort and the RosNodeParams are empty."); - } - else { - createSubscriber(params.default_port_value); - } - } -} - -template inline - bool RosTopicSubSharedNode::createSubscriber(const std::string& topic_name) -{ - if(topic_name.empty()) - { - throw RuntimeError("topic_name is empty"); - } - if(sub_instance_) - { - throw RuntimeError("Can't call createSubscriber more than once"); - } - - // find SubscriberInstance in the registry - std::unique_lock lk(registryMutex()); - subscriber_key_ = std::string(node_->get_fully_qualified_name()) + "/" + topic_name; - - auto& registry_ = getRegistry(); - auto it = registry_.find(subscriber_key_); - - if(!shared_resource_initialized && it == registry_.end()) - { - it = registry_.insert( {subscriber_key_, std::make_shared() }).first; - sub_instance_ = it->second; - sub_instance_->init(node_, topic_name); - - RCLCPP_INFO(logger(), - "Node [%s] created Subscriber to topic [%s]", - name().c_str(), topic_name.c_str() ); - - shared_resource_initialized = true; - } - else if(it == registry_.end()) - { - throw RuntimeError("Class shared resource has been initialized but is not in the registry!"); - } - else - { - sub_instance_ = it->second; - RCLCPP_INFO(logger(), - "Node [%s] retrieved Subscriber to topic [%s]", - name().c_str(), topic_name.c_str() ); - } - - // add "this" as received of the broadcaster - signal_connection_ = sub_instance_->broadcaster.connect( - [this](const std::shared_ptr msg) { last_msg_ = msg; } ); - - topic_name_ = topic_name; - return true; -} - - -template inline - NodeStatus RosTopicSubSharedNode::tick() -{ - // First, check if the subscriber_ is valid and that the name of the - // topic_name in the port didn't change. - // otherwise, create a new subscriber - if(!sub_instance_) - { - std::string topic_name; - getInput("topic_name", topic_name); - createSubscriber(topic_name); - } - - auto CheckStatus = [](NodeStatus status) - { - if( !isStatusCompleted(status) ) - { - throw std::logic_error("RosTopicSubSharedNode: the callback must return" - "either SUCCESS or FAILURE"); - } - return status; - }; - - // RCLCPP_INFO(rclcpp::get_logger("test"), "Tick, before spinsome"); - sub_instance_->callback_group_executor.spin_some(); - auto status = CheckStatus (onTick(last_msg_)); - last_msg_ = nullptr; - - return status; -} - -template -bool RosTopicSubSharedNode::shared_resource_initialized = false; - -// template -// typename std::shared_ptr> RosTopicPubSharedNode::publisher_ = nullptr; - -} // namespace BT - From b968e8624851d107b554b8be0b3ab1fa5abd549a Mon Sep 17 00:00:00 2001 From: Rebecca Williams Date: Wed, 31 Jan 2024 15:09:43 +0000 Subject: [PATCH 16/18] tidy up --- .../behaviortree_ros2/bt_action_node.hpp | 26 ++++++++----------- .../behaviortree_ros2/bt_service_node.hpp | 10 +++---- .../behaviortree_ros2/bt_topic_pub_node.hpp | 7 +++-- 3 files changed, 19 insertions(+), 24 deletions(-) diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp index 7640a79..e7473ad 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp @@ -24,7 +24,6 @@ #include "rclcpp_action/rclcpp_action.hpp" #include "behaviortree_ros2/ros_node_params.hpp" -#include "behaviortree_ros2/bt_action_error_code.hpp" namespace BT { @@ -95,14 +94,16 @@ class RosActionNode : public BT::ActionNodeBase virtual ~RosActionNode() { - if (action_client_instance_) { + if (action_client_instance_) + { action_client_instance_.reset(); - std::unique_lock lk(registryMutex()); + std::unique_lock lk(getMutex()); auto& registry = getRegistry(); auto it = registry.find(action_client_key_); - if (it != registry.end() && it->second.use_count() <= 1){ + if (it != registry.end() && it->second.use_count() <= 1) + { registry.erase(it); - RCLCPP_INFO(logger(), "Remove action client [%s]", action_name_.c_str()); + RCLCPP_INFO(logger(), "Removed action client [%s]", action_name_.c_str()); } } } @@ -193,13 +194,7 @@ class RosActionNode : public BT::ActionNodeBase typename ActionClient::SendGoalOptions goal_options; }; - static std::mutex& registryMutex() - { - static std::mutex sub_mutex; - return sub_mutex; - } - - static std::mutex& actionClientMutex() + static std::mutex& getMutex() { static std::mutex action_client_mutex; return action_client_mutex; @@ -213,7 +208,8 @@ class RosActionNode : public BT::ActionNodeBase // contains the fully-qualified name of the node and the name of the topic static std::unordered_map>& getRegistry() { - static std::unordered_map> action_clients_registry; + static std::unordered_map> + action_clients_registry; return action_clients_registry; } @@ -303,7 +299,7 @@ template inline throw RuntimeError("action_name is empty"); } - std::unique_lock lk(registryMutex()); + std::unique_lock lk(getMutex()); action_client_key_ = std::string(node_->get_fully_qualified_name()) + "/" + action_name; auto& registry = getRegistry(); @@ -425,7 +421,7 @@ template inline if (status() == NodeStatus::RUNNING) { - std::unique_lock lock(actionClientMutex()); + std::unique_lock lock(getMutex()); action_client_instance_->callback_group_executor.spin_some(); // FIRST case: check if the goal request has a timeout diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp index c405d98..98d699c 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp @@ -88,7 +88,7 @@ class RosServiceNode : public BT::ActionNodeBase srv_instance_.reset(); std::unique_lock lk(registryMutex()); auto& registry = getRegistry(); - auto it = registry.find(service_clientkey_); + auto it = registry.find(service_client_key_); if (it != registry.end() && it->second.use_count() <= 1){ registry.erase(it); RCLCPP_INFO(logger(), "Remove service [%s]", service_name_.c_str()); @@ -188,7 +188,7 @@ class RosServiceNode : public BT::ActionNodeBase bool service_name_may_change_ = false; const std::chrono::milliseconds service_timeout_; const std::chrono::milliseconds wait_for_service_timeout_; - std::string service_clientkey_; + std::string service_client_key_; rclcpp::Logger logger() { @@ -269,13 +269,13 @@ template inline } std::unique_lock lk(registryMutex()); - service_clientkey_ = std::string(node_->get_fully_qualified_name()) + "/" + service_name; + service_client_key_ = std::string(node_->get_fully_qualified_name()) + "/" + service_name; auto& registry = getRegistry(); - auto it = registry.find(service_clientkey_); + auto it = registry.find(service_client_key_); if (it == registry.end()) { - it = registry.insert({service_clientkey_, std::make_shared()}).first; + it = registry.insert({service_client_key_, std::make_shared()}).first; srv_instance_ = it->second; srv_instance_->init(node_, service_name); diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_pub_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_pub_node.hpp index 8694fbf..02c2f63 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_topic_pub_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_topic_pub_node.hpp @@ -104,9 +104,9 @@ class RosTopicPubNode : public BT::ConditionNode { void init(std::shared_ptr node, const std::string& topic_name) { - publisher_ = node->create_publisher(topic_name, 1); + publisher = node->create_publisher(topic_name, 1); } - std::shared_ptr publisher_; + std::shared_ptr publisher; }; static std::mutex& registryMutex() @@ -244,8 +244,7 @@ template inline { return NodeStatus::FAILURE; } - pub_instance_->publisher_->publish(msg); - // publisher_->publish(msg); + pub_instance_->publisher->publish(msg); return NodeStatus::SUCCESS; } From e57d8c36ec3bb2c9f481b700c290808be0cbb606 Mon Sep 17 00:00:00 2001 From: Rebecca Williams Date: Wed, 31 Jan 2024 15:11:55 +0000 Subject: [PATCH 17/18] tidy up examples --- btcpp_ros2_samples/CMakeLists.txt | 19 ---- .../src/publisher_test_shared.cpp | 68 -------------- .../src/service_shared_test.cpp | 84 ----------------- .../src/sleep_action_shared.cpp | 33 ------- .../src/sleep_action_shared.hpp | 27 ------ btcpp_ros2_samples/src/sleep_client.cpp | 2 +- .../src/sleep_client_shared.cpp | 91 ------------------- .../src/subscriber_shared_test.cpp | 71 --------------- btcpp_ros2_samples/src/subscriber_test.cpp | 2 +- 9 files changed, 2 insertions(+), 395 deletions(-) delete mode 100644 btcpp_ros2_samples/src/publisher_test_shared.cpp delete mode 100644 btcpp_ros2_samples/src/service_shared_test.cpp delete mode 100644 btcpp_ros2_samples/src/sleep_action_shared.cpp delete mode 100644 btcpp_ros2_samples/src/sleep_action_shared.hpp delete mode 100644 btcpp_ros2_samples/src/sleep_client_shared.cpp delete mode 100644 btcpp_ros2_samples/src/subscriber_shared_test.cpp diff --git a/btcpp_ros2_samples/CMakeLists.txt b/btcpp_ros2_samples/CMakeLists.txt index e440cab..3ccba2a 100644 --- a/btcpp_ros2_samples/CMakeLists.txt +++ b/btcpp_ros2_samples/CMakeLists.txt @@ -23,12 +23,6 @@ add_executable(sleep_client src/sleep_client.cpp) ament_target_dependencies(sleep_client ${THIS_PACKAGE_DEPS}) - -add_executable(sleep_client_shared - src/sleep_action_shared.cpp - src/sleep_client_shared.cpp) -ament_target_dependencies(sleep_client_shared ${THIS_PACKAGE_DEPS}) - ###################################################### # Build a client that call the sleep action (Plugin version) @@ -51,17 +45,11 @@ ament_target_dependencies(sleep_server ${THIS_PACKAGE_DEPS}) add_executable(subscriber_test src/subscriber_test.cpp) ament_target_dependencies(subscriber_test ${THIS_PACKAGE_DEPS}) -add_executable(subscriber_shared_test src/subscriber_shared_test.cpp) -ament_target_dependencies(subscriber_shared_test ${THIS_PACKAGE_DEPS}) - ###################################################### # Build publisher_tests add_executable(publisher_test src/publisher_test.cpp) ament_target_dependencies(publisher_test ${THIS_PACKAGE_DEPS}) -add_executable(publisher_shared_test src/publisher_test_shared.cpp) -ament_target_dependencies(publisher_shared_test ${THIS_PACKAGE_DEPS}) - ###################################################### # Build service_tests add_executable(add_two_ints_server src/add_two_ints_server.cpp) @@ -70,25 +58,18 @@ ament_target_dependencies(add_two_ints_server ${THIS_PACKAGE_DEPS}) add_executable(service_test src/service_test.cpp) ament_target_dependencies(service_test ${THIS_PACKAGE_DEPS}) -add_executable(service_shared_test src/service_shared_test.cpp) -ament_target_dependencies(service_shared_test ${THIS_PACKAGE_DEPS}) - ###################################################### # INSTALL install(TARGETS sleep_client - sleep_client_shared sleep_client_dyn sleep_server sleep_plugin subscriber_test - subscriber_shared_test publisher_test - publisher_shared_test add_two_ints_server service_test - service_shared_test DESTINATION lib/${PROJECT_NAME} ) diff --git a/btcpp_ros2_samples/src/publisher_test_shared.cpp b/btcpp_ros2_samples/src/publisher_test_shared.cpp deleted file mode 100644 index 3a6072f..0000000 --- a/btcpp_ros2_samples/src/publisher_test_shared.cpp +++ /dev/null @@ -1,68 +0,0 @@ -#include "behaviortree_ros2/bt_topic_pub_shared_node.hpp" -#include -#include - -using namespace BT; - -class SendString: public RosTopicPubSharedNode -{ -public: - SendString(const std::string& name, - const NodeConfig& conf, - const RosNodeParams& params) - : RosTopicPubSharedNode(name, conf, params) - {} - - static BT::PortsList providedPorts() - { - BT::PortsList addition; - BT::PortsList basic = { - InputPort("message", "Hello World!", "Message to send") - }; - basic.insert(addition.begin(), addition.end()); - - return providedBasicPorts(basic); - } - - bool setMessage(std_msgs::msg::String& msg) - { - std::string user_message = ""; - getInput("message", user_message); - msg.data = user_message; - return true; - } -}; - - // 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("publisher_shared_test"); - - BehaviorTreeFactory factory; - - RosNodeParams params; - params.nh = nh; - params.default_port_value = "btcpp_string"; - factory.registerNodeType("SendString", params); - - auto tree = factory.createTreeFromText(xml_text); - - while(rclcpp::ok()) - { - tree.tickWhileRunning(); - } - return 0; -} diff --git a/btcpp_ros2_samples/src/service_shared_test.cpp b/btcpp_ros2_samples/src/service_shared_test.cpp deleted file mode 100644 index a22d148..0000000 --- a/btcpp_ros2_samples/src/service_shared_test.cpp +++ /dev/null @@ -1,84 +0,0 @@ -#include "behaviortree_ros2/bt_service_shared_node.hpp" -#include "btcpp_ros2_interfaces/srv/add_two_ints.hpp" - -using namespace BT; - -class AddTwoIntsClient: public RosServiceSharedNode -{ -public: - AddTwoIntsClient(const std::string& name, - const NodeConfig& conf, - const RosNodeParams& params) - : RosServiceSharedNode(name, conf, params) - {} - - static PortsList providedPorts() - { - BT::PortsList addition; - BT::PortsList basic = { - InputPort("a", 12, "first integer to add"), - InputPort("b", 12, "second integer to add") - }; - basic.insert(addition.begin(), addition.end()); - return providedBasicPorts(basic); - } - - bool setRequest(Request::SharedPtr& req) override - { - getInput("a", a); - getInput("b", b); - req->a = a; - req->b = b; - - return true; - } - - virtual BT::NodeStatus onResponseReceived(const Response::SharedPtr& res) override - { - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Result of %d + %d = %ld", - a, b, res->sum); - return BT::NodeStatus::SUCCESS; - } - - virtual BT::NodeStatus onFailure(ServiceNodeErrorCode error_code) override - { - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "AddTwoIntsClient failed with error code %d", error_code); - return BT::NodeStatus::FAILURE; - } -private: - int a; - int b; -}; - - // 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("service_test"); - - BehaviorTreeFactory factory; - - RosNodeParams params; - params.nh = nh; - params.default_port_value = "btcpp_service"; - factory.registerNodeType("AddTwoIntsClient", params); - - auto tree = factory.createTreeFromText(xml_text); - - while(rclcpp::ok()) - { - tree.tickWhileRunning(); - } - return 0; -} diff --git a/btcpp_ros2_samples/src/sleep_action_shared.cpp b/btcpp_ros2_samples/src/sleep_action_shared.cpp deleted file mode 100644 index af439bd..0000000 --- a/btcpp_ros2_samples/src/sleep_action_shared.cpp +++ /dev/null @@ -1,33 +0,0 @@ -#include "sleep_action_shared.hpp" -#include "behaviortree_ros2/plugins.hpp" - -bool SleepAction::setGoal(RosActionSharedNode::Goal &goal) -{ - RCLCPP_INFO(rclcpp::get_logger("test"), "%s, set goal", name().c_str()); - auto timeout = getInput("msec"); - goal.msec_timeout = timeout.value(); - return true; -} - -NodeStatus SleepAction::onResultReceived(const RosActionSharedNode::WrappedResult &wr) -{ - RCLCPP_INFO( node_->get_logger(), "%s: onResultReceived. Done = %s", name().c_str(), - wr.result->done ? "true" : "false" ); - - return wr.result->done ? NodeStatus::SUCCESS : NodeStatus::FAILURE; -} - -NodeStatus SleepAction::onFailure(ActionNodeErrorCode error) -{ - RCLCPP_ERROR( node_->get_logger(), "%s: onFailure with error: %s", name().c_str(), toStr(error) ); - return NodeStatus::FAILURE; -} - -void SleepAction::onHalt() -{ - RCLCPP_INFO( node_->get_logger(), "%s: onHalt", name().c_str() ); -} - -// Plugin registration. -// The class SleepAction will self register with name "Sleep". -CreateRosNodePlugin(SleepAction, "Sleep"); diff --git a/btcpp_ros2_samples/src/sleep_action_shared.hpp b/btcpp_ros2_samples/src/sleep_action_shared.hpp deleted file mode 100644 index b8c3ccc..0000000 --- a/btcpp_ros2_samples/src/sleep_action_shared.hpp +++ /dev/null @@ -1,27 +0,0 @@ -#include "behaviortree_ros2/bt_action_shared_node.hpp" -#include "btcpp_ros2_interfaces/action/sleep.hpp" - -using namespace BT; - -class SleepAction: public RosActionSharedNode -{ -public: - SleepAction(const std::string& name, - const NodeConfig& conf, - const RosNodeParams& params) - : RosActionSharedNode(name, conf, params) - {} - - static BT::PortsList providedPorts() - { - return providedBasicPorts({InputPort("msec")}); - } - - bool setGoal(Goal& goal) override; - - void onHalt() override; - - BT::NodeStatus onResultReceived(const WrappedResult& wr) override; - - virtual BT::NodeStatus onFailure(ActionNodeErrorCode error) override; -}; diff --git a/btcpp_ros2_samples/src/sleep_client.cpp b/btcpp_ros2_samples/src/sleep_client.cpp index 5899e40..97c7929 100644 --- a/btcpp_ros2_samples/src/sleep_client.cpp +++ b/btcpp_ros2_samples/src/sleep_client.cpp @@ -50,7 +50,7 @@ class PrintValue : public BT::SyncActionNode - + diff --git a/btcpp_ros2_samples/src/sleep_client_shared.cpp b/btcpp_ros2_samples/src/sleep_client_shared.cpp deleted file mode 100644 index e1793e2..0000000 --- a/btcpp_ros2_samples/src/sleep_client_shared.cpp +++ /dev/null @@ -1,91 +0,0 @@ -#include "behaviortree_ros2/bt_action_shared_node.hpp" -#include "rclcpp/rclcpp.hpp" -#include "rclcpp/executors.hpp" - -#include "behaviortree_ros2/plugins.hpp" - -#ifndef USE_SLEEP_PLUGIN -#include "sleep_action_shared.hpp" -#endif - -using namespace BT; - -//------------------------------------------------------------- -// Simple Action to print a number -//------------------------------------------------------------- - -class PrintValue : public BT::SyncActionNode -{ -public: - PrintValue(const std::string& name, const BT::NodeConfig& 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") }; - } -}; - -//----------------------------------------------------- - - // 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"); - - RosNodeParams params; - params.nh = nh; - params.default_port_value = "sleep_service"; - -#ifdef USE_SLEEP_PLUGIN - RegisterRosNode(factory, "../lib/libsleep_action_plugin.so", params); -#else - factory.registerNodeType("SleepAction", params); -#endif - - auto tree = factory.createTreeFromText(xml_text); - - while(rclcpp::ok()) - { - tree.tickWhileRunning(); - } - - return 0; -} diff --git a/btcpp_ros2_samples/src/subscriber_shared_test.cpp b/btcpp_ros2_samples/src/subscriber_shared_test.cpp deleted file mode 100644 index 8f9c6de..0000000 --- a/btcpp_ros2_samples/src/subscriber_shared_test.cpp +++ /dev/null @@ -1,71 +0,0 @@ -#include "behaviortree_ros2/bt_topic_sub_shared_node.hpp" -#include - -using namespace BT; - -class ReceiveString: public RosTopicSubSharedNode -{ -public: - ReceiveString(const std::string& name, - const NodeConfig& conf, - const RosNodeParams& params) - : RosTopicSubSharedNode(name, conf, params) - {} - - static BT::PortsList providedPorts() - { - BT::PortsList addition; - BT::PortsList basic = {}; - basic.insert(addition.begin(), addition.end()); - - return providedBasicPorts(basic); - } - - NodeStatus onTick(const std::shared_ptr& last_msg) override - { - // RCLCPP_INFO(rclcpp::get_logger("test"), "%s, subscriber pointer: %p", name().c_str(), reinterpret_cast(sub_instance_->subscriber.get())); - if(last_msg) // empty if no new message received, since the last tick - { - RCLCPP_INFO(logger(), "[%s] new message: %s", name().c_str(), last_msg->data.c_str()); - } - return NodeStatus::SUCCESS; - } -}; - - // 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("subscriber_shared_test"); - - BehaviorTreeFactory factory; - - RosNodeParams params; - params.nh = nh; - params.default_port_value = "btcpp_string"; - factory.registerNodeType("ReceiveString", params); - - auto tree = factory.createTreeFromText(xml_text); - - while(rclcpp::ok()) - { - tree.tickWhileRunning(); - } - - return 0; -} diff --git a/btcpp_ros2_samples/src/subscriber_test.cpp b/btcpp_ros2_samples/src/subscriber_test.cpp index 1eb208c..b576d3f 100644 --- a/btcpp_ros2_samples/src/subscriber_test.cpp +++ b/btcpp_ros2_samples/src/subscriber_test.cpp @@ -37,7 +37,7 @@ class ReceiveString: public RosTopicSubNode - + From fa7261c856468aabb56c14bc940479527b150bff Mon Sep 17 00:00:00 2001 From: Rebecca Williams Date: Wed, 31 Jan 2024 15:48:31 +0000 Subject: [PATCH 18/18] remove settings.json --- .vscode/settings.json | 59 ------------------------------------------- 1 file changed, 59 deletions(-) delete mode 100644 .vscode/settings.json diff --git a/.vscode/settings.json b/.vscode/settings.json deleted file mode 100644 index 165f327..0000000 --- a/.vscode/settings.json +++ /dev/null @@ -1,59 +0,0 @@ -{ - "files.associations": { - "array": "cpp", - "atomic": "cpp", - "bit": "cpp", - "*.tcc": "cpp", - "cctype": "cpp", - "chrono": "cpp", - "clocale": "cpp", - "cmath": "cpp", - "codecvt": "cpp", - "compare": "cpp", - "concepts": "cpp", - "cstdarg": "cpp", - "cstddef": "cpp", - "cstdint": "cpp", - "cstdio": "cpp", - "cstdlib": "cpp", - "ctime": "cpp", - "cwchar": "cpp", - "cwctype": "cpp", - "deque": "cpp", - "string": "cpp", - "unordered_map": "cpp", - "vector": "cpp", - "exception": "cpp", - "algorithm": "cpp", - "functional": "cpp", - "iterator": "cpp", - "memory": "cpp", - "memory_resource": "cpp", - "numeric": "cpp", - "random": "cpp", - "ratio": "cpp", - "string_view": "cpp", - "system_error": "cpp", - "tuple": "cpp", - "type_traits": "cpp", - "utility": "cpp", - "initializer_list": "cpp", - "iomanip": "cpp", - "iosfwd": "cpp", - "istream": "cpp", - "limits": "cpp", - "new": "cpp", - "numbers": "cpp", - "ostream": "cpp", - "semaphore": "cpp", - "sstream": "cpp", - "stdexcept": "cpp", - "stop_token": "cpp", - "streambuf": "cpp", - "thread": "cpp", - "typeinfo": "cpp", - "iostream": "cpp", - "condition_variable": "cpp", - "mutex": "cpp" - } -}