diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp index 8e4d49f..e7473ad 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp @@ -27,7 +27,6 @@ namespace BT { - enum ActionNodeErrorCode { SERVER_UNREACHABLE, @@ -93,7 +92,21 @@ 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(getMutex()); + 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(), "Removed action client [%s]", action_name_.c_str()); + } + } + } /** * @brief Any subclass of RosActionNode that has ports must implement a @@ -166,19 +179,49 @@ 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& getMutex() + { + 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_; @@ -256,17 +299,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(getMutex()); + 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; } @@ -274,14 +328,20 @@ template inline template inline NodeStatus RosActionNode::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_)) + 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); } @@ -352,7 +412,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; @@ -360,7 +421,8 @@ template inline if (status() == NodeStatus::RUNNING) { - callback_group_executor_.spin_some(); + std::unique_lock lock(getMutex()); + action_client_instance_->callback_group_executor.spin_some(); // FIRST case: check if the goal request has a timeout if( !goal_received_ ) @@ -368,7 +430,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 ) @@ -429,26 +491,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 diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp index 66477e3..98d699c 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp @@ -25,7 +25,6 @@ namespace BT { - enum ServiceNodeErrorCode { SERVICE_UNREACHABLE, @@ -82,7 +81,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_client_key_); + 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 @@ -133,24 +145,56 @@ class RosServiceNode : public BT::ActionNodeBase * It must return either SUCCESS or FAILURE. */ virtual BT::NodeStatus onFailure(ServiceNodeErrorCode /*error*/) - { + { return NodeStatus::FAILURE; } 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_client_key_; + 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_; @@ -158,6 +202,7 @@ class RosServiceNode : public BT::ActionNodeBase bool response_received_; typename Response::SharedPtr response_; + bool createClient(const std::string &service_name); }; @@ -217,21 +262,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_client_key_ = std::string(node_->get_fully_qualified_name()) + "/" + service_name; - bool found = service_client_->wait_for_service(wait_for_service_timeout_); + auto& registry = getRegistry(); + auto it = registry.find(service_client_key_); + if (it == registry.end()) + { + it = registry.insert({service_client_key_, 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 = 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; } @@ -239,14 +300,19 @@ template inline template inline NodeStatus RosServiceNode::tick() { - // First, check if the service_client_ is valid and that the name of the + 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_)) + 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); } @@ -278,7 +344,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; @@ -286,7 +352,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_ ) @@ -294,7 +360,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) { 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..02c2f63 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,40 @@ 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); }; @@ -110,7 +148,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()) @@ -157,23 +195,45 @@ 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; } 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 - 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); } @@ -184,7 +244,7 @@ template inline { return NodeStatus::FAILURE; } - publisher_->publish(msg); + pub_instance_->publisher->publish(msg); return NodeStatus::SUCCESS; } 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/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 f2bb870..3ccba2a 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,25 @@ 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}) + +###################################################### +# Build publisher_tests +add_executable(publisher_test src/publisher_test.cpp) +ament_target_dependencies(publisher_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 @@ -54,6 +67,9 @@ install(TARGETS sleep_server sleep_plugin subscriber_test + publisher_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/publisher_test.cpp b/btcpp_ros2_samples/src/publisher_test.cpp new file mode 100644 index 0000000..e8aa66e --- /dev/null +++ b/btcpp_ros2_samples/src/publisher_test.cpp @@ -0,0 +1,68 @@ +#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) + { + 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/service_test.cpp b/btcpp_ros2_samples/src/service_test.cpp new file mode 100644 index 0000000..93235fe --- /dev/null +++ b/btcpp_ros2_samples/src/service_test.cpp @@ -0,0 +1,84 @@ +#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("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.cpp b/btcpp_ros2_samples/src/sleep_action.cpp index a838d74..284764c 100644 --- a/btcpp_ros2_samples/src/sleep_action.cpp +++ b/btcpp_ros2_samples/src/sleep_action.cpp @@ -10,9 +10,8 @@ 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" ); - return wr.result->done ? NodeStatus::SUCCESS : NodeStatus::FAILURE; } diff --git a/btcpp_ros2_samples/src/sleep_client.cpp b/btcpp_ros2_samples/src/sleep_client.cpp index 2290f7e..97c7929 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; @@ -43,9 +44,10 @@ class PrintValue : public BT::SyncActionNode static const char* xml_text = R"( + - + @@ -54,6 +56,7 @@ class PrintValue : public BT::SyncActionNode + )"; @@ -79,7 +82,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(); } diff --git a/btcpp_ros2_samples/src/subscriber_test.cpp b/btcpp_ros2_samples/src/subscriber_test.cpp index 195f1b9..b576d3f 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 - - - + +