From bb5535e47921a898fd60a7431300e9ca19ec3533 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Wed, 15 May 2024 18:07:52 +0200 Subject: [PATCH] Change the way default port is passed, to allow namespaces --- .../behaviortree_ros2/bt_action_node.hpp | 70 +++++++++---------- .../behaviortree_ros2/bt_service_node.hpp | 66 ++++++++--------- .../behaviortree_ros2/ros_node_params.hpp | 7 ++ .../tree_execution_server.hpp | 9 +++ .../src/tree_execution_server.cpp | 5 ++ btcpp_ros2_samples/CMakeLists.txt | 12 ++++ btcpp_ros2_samples/src/bool_client.cpp | 45 ++++++++++++ btcpp_ros2_samples/src/bool_server.cpp | 37 ++++++++++ btcpp_ros2_samples/src/set_bool_node.cpp | 41 +++++++++++ btcpp_ros2_samples/src/set_bool_node.hpp | 51 ++++++++++++++ 10 files changed, 270 insertions(+), 73 deletions(-) create mode 100644 btcpp_ros2_samples/src/bool_client.cpp create mode 100644 btcpp_ros2_samples/src/bool_server.cpp create mode 100644 btcpp_ros2_samples/src/set_bool_node.cpp create mode 100644 btcpp_ros2_samples/src/set_bool_node.hpp diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp index e2d4f15..05c664d 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp @@ -108,8 +108,7 @@ class RosActionNode : public BT::ActionNodeBase */ static PortsList providedBasicPorts(PortsList addition) { - PortsList basic = { InputPort("action_name", "__default__placeholder__", - "Action server name") }; + PortsList basic = { InputPort("action_name", "", "Action server name") }; basic.insert(addition.begin(), addition.end()); return basic; } @@ -164,9 +163,12 @@ class RosActionNode : public BT::ActionNodeBase void cancelGoal(); /// The default halt() implementation will call cancelGoal if necessary. - void halt() override final; + void halt() override; - NodeStatus tick() override final; + NodeStatus tick() override; + + /// Can be used to change the name of the action programmatically + void setActionName(const std::string& action_name); protected: struct ActionClientInstance @@ -216,7 +218,7 @@ class RosActionNode : public BT::ActionNodeBase std::weak_ptr node_; std::shared_ptr client_instance_; std::string action_name_; - bool action_name_may_change_ = false; + bool action_name_should_be_checked_ = false; const std::chrono::milliseconds server_timeout_; const std::chrono::milliseconds wait_for_server_timeout_; std::string action_client_key_; @@ -265,44 +267,23 @@ inline RosActionNode::RosActionNode(const std::string& instance_name, auto portIt = config().input_ports.find("action_name"); if(portIt != config().input_ports.end()) { - const std::string& bb_action_name = portIt->second; + const std::string& bb_service_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(isBlackboardPointer(bb_service_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); + // unknown value at construction time. Postpone to tick + action_name_should_be_checked_ = true; } - else + else if(!bb_service_name.empty()) { - action_name_may_change_ = true; - // createClient will be invoked in the first tick(). + // "hard-coded" name in the bb_service_name. Use it. + createClient(bb_service_name); } } - else + // no port value or it is empty. Use the default value + if(!client_instance_ && !params.default_port_value.empty()) { - 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); - } + createClient(params.default_port_value); } } @@ -347,6 +328,13 @@ inline bool RosActionNode::createClient(const std::string& action_name) return found; } +template +inline void RosActionNode::setActionName(const std::string& action_name) +{ + action_name_ = action_name; + createClient(action_name); +} + template inline NodeStatus RosActionNode::tick() { @@ -359,7 +347,8 @@ inline NodeStatus RosActionNode::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(!client_instance_ || (status() == NodeStatus::IDLE && action_name_may_change_)) + if(!client_instance_ || + (status() == NodeStatus::IDLE && action_name_should_be_checked_)) { std::string action_name; getInput("action_name", action_name); @@ -368,6 +357,13 @@ inline NodeStatus RosActionNode::tick() createClient(action_name); } } + + if(!client_instance_) + { + throw BT::RuntimeError("RosActionNode: no client was specified neither as default or " + "in the ports"); + } + auto& action_client = client_instance_->action_client; //------------------------------------------ diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp index f3a6203..77958cd 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp @@ -96,8 +96,7 @@ class RosServiceNode : public BT::ActionNodeBase */ static PortsList providedBasicPorts(PortsList addition) { - PortsList basic = { InputPort("service_name", "__default__placeholder__", - "Service name") }; + PortsList basic = { InputPort("service_name", "", "Service name") }; basic.insert(addition.begin(), addition.end()); return basic; } @@ -111,7 +110,7 @@ class RosServiceNode : public BT::ActionNodeBase return providedBasicPorts({}); } - NodeStatus tick() override final; + NodeStatus tick() override; /// The default halt() implementation. void halt() override; @@ -157,6 +156,9 @@ class RosServiceNode : public BT::ActionNodeBase return action_client_mutex; } + // method to set the service name programmatically + void setServiceName(const std::string& service_name); + rclcpp::Logger logger() { if(auto node = node_.lock()) @@ -186,7 +188,7 @@ class RosServiceNode : public BT::ActionNodeBase std::weak_ptr node_; std::string service_name_; - bool service_name_may_change_ = false; + bool service_name_should_be_checked_ = false; const std::chrono::milliseconds service_timeout_; const std::chrono::milliseconds wait_for_service_timeout_; @@ -233,51 +235,30 @@ inline RosServiceNode::RosServiceNode(const std::string& instance_name, { const std::string& bb_service_name = portIt->second; - if(bb_service_name.empty() || bb_service_name == "__default__placeholder__") + if(isBlackboardPointer(bb_service_name)) { - 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); - } + // unknown value at construction time. postpone to tick + service_name_should_be_checked_ = true; } - else if(!isBlackboardPointer(bb_service_name)) + else if(!bb_service_name.empty()) { - // 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. + // "hard-coded" name in the bb_service_name. Use it. createClient(bb_service_name); } - else - { - service_name_may_change_ = true; - // createClient will be invoked in the first tick(). - } } - else + // no port value or it is empty. Use the default port value + if(!srv_instance_ && !params.default_port_value.empty()) { - 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); - } + createClient(params.default_port_value); } } template inline bool RosServiceNode::createClient(const std::string& service_name) { - if(service_name.empty()) + if(service_name.empty() || service_name == "__default__placeholder__") { - throw RuntimeError("service_name is empty"); + throw RuntimeError("service_name is empty or invalid"); } std::unique_lock lk(getMutex()); @@ -314,6 +295,13 @@ inline bool RosServiceNode::createClient(const std::string& service_name) return found; } +template +inline void RosServiceNode::setServiceName(const std::string& service_name) +{ + service_name_ = service_name; + createClient(service_name); +} + template inline NodeStatus RosServiceNode::tick() { @@ -326,7 +314,7 @@ inline NodeStatus RosServiceNode::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(!srv_instance_ || (status() == NodeStatus::IDLE && service_name_may_change_)) + if(!srv_instance_ || (status() == NodeStatus::IDLE && service_name_should_be_checked_)) { std::string service_name; getInput("service_name", service_name); @@ -336,6 +324,12 @@ inline NodeStatus RosServiceNode::tick() } } + if(!srv_instance_) + { + throw BT::RuntimeError("RosServiceNode: no service client was specified neither as " + "default or in the ports"); + } + auto CheckStatus = [](NodeStatus status) { if(!isStatusCompleted(status)) { diff --git a/behaviortree_ros2/include/behaviortree_ros2/ros_node_params.hpp b/behaviortree_ros2/include/behaviortree_ros2/ros_node_params.hpp index 484429b..d9e1de7 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/ros_node_params.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/ros_node_params.hpp @@ -40,6 +40,13 @@ struct RosNodeParams std::chrono::milliseconds server_timeout = std::chrono::milliseconds(1000); // timeout used when detecting the server the first time std::chrono::milliseconds wait_for_server_timeout = std::chrono::milliseconds(500); + + RosNodeParams() = default; + RosNodeParams(std::shared_ptr node) : nh(node) + {} + RosNodeParams(std::shared_ptr node, const std::string& port_name) + : nh(node), default_port_value(port_name) + {} }; } // namespace BT diff --git a/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp b/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp index 1760f44..f472cfd 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp @@ -76,6 +76,15 @@ class TreeExecutionServer BT::BehaviorTreeFactory& factory(); protected: + /** + * @brief Callback invoked when a goal is received and before the tree is created. + * If it returns false, the goal will be rejected. + */ + virtual bool onGoalReceived(const std::string& tree_name, const std::string& payload) + { + return true; + } + /** * @brief Callback invoked after the tree is created. * It can be used, for instance, to initialize a logger or the global blackboard. diff --git a/behaviortree_ros2/src/tree_execution_server.cpp b/behaviortree_ros2/src/tree_execution_server.cpp index 80c112a..c7dd46b 100644 --- a/behaviortree_ros2/src/tree_execution_server.cpp +++ b/behaviortree_ros2/src/tree_execution_server.cpp @@ -121,6 +121,11 @@ TreeExecutionServer::handle_goal(const rclcpp_action::GoalUUID& /* uuid */, { RCLCPP_INFO(kLogger, "Received goal request to execute Behavior Tree: %s", goal->target_tree.c_str()); + + if(!onGoalReceived(goal->target_tree, goal->payload)) + { + return rclcpp_action::GoalResponse::REJECT; + } return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } diff --git a/btcpp_ros2_samples/CMakeLists.txt b/btcpp_ros2_samples/CMakeLists.txt index 785d674..0ff08e5 100644 --- a/btcpp_ros2_samples/CMakeLists.txt +++ b/btcpp_ros2_samples/CMakeLists.txt @@ -9,10 +9,12 @@ find_package(ament_cmake REQUIRED) find_package(behaviortree_ros2 REQUIRED) find_package(btcpp_ros2_interfaces REQUIRED) find_package(std_msgs REQUIRED) +find_package(std_srvs REQUIRED) set(THIS_PACKAGE_DEPS behaviortree_ros2 std_msgs + std_srvs btcpp_ros2_interfaces ) ###################################################### @@ -50,6 +52,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}) +###################################################### +# the SetBool test +add_executable(bool_client src/bool_client.cpp src/set_bool_node.cpp) +ament_target_dependencies(bool_client ${THIS_PACKAGE_DEPS}) + +add_executable(bool_server src/bool_server.cpp ) +ament_target_dependencies(bool_server ${THIS_PACKAGE_DEPS}) + ###################################################### # INSTALL @@ -60,6 +70,8 @@ install(TARGETS sleep_plugin subscriber_test sample_bt_executor + bool_client + bool_server DESTINATION lib/${PROJECT_NAME} ) diff --git a/btcpp_ros2_samples/src/bool_client.cpp b/btcpp_ros2_samples/src/bool_client.cpp new file mode 100644 index 0000000..dc00710 --- /dev/null +++ b/btcpp_ros2_samples/src/bool_client.cpp @@ -0,0 +1,45 @@ +#include "behaviortree_ros2/bt_action_node.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/executors.hpp" + +#include "set_bool_node.hpp" + +using namespace BT; + +static const char* xml_text = R"( + + + + + + + + + + + + )"; + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + auto nh = std::make_shared("bool_client"); + + BehaviorTreeFactory factory; + + // version with default port + factory.registerNodeType("SetBoolA", BT::RosNodeParams(nh, "robotA/" + "set_bool")); + + // version without default port + factory.registerNodeType("SetBool", BT::RosNodeParams(nh)); + + // namespace version + factory.registerNodeType("SetRobotBool", nh, "set_bool"); + + auto tree = factory.createTreeFromText(xml_text); + + tree.tickWhileRunning(); + + return 0; +} diff --git a/btcpp_ros2_samples/src/bool_server.cpp b/btcpp_ros2_samples/src/bool_server.cpp new file mode 100644 index 0000000..91c77e8 --- /dev/null +++ b/btcpp_ros2_samples/src/bool_server.cpp @@ -0,0 +1,37 @@ +#include "rclcpp/rclcpp.hpp" +#include "std_srvs/srv/set_bool.hpp" + +#include + +void CallbackA(const std::shared_ptr request, + std::shared_ptr response) +{ + response->success = request->data; + auto logger = rclcpp::get_logger("rclcpp"); + RCLCPP_INFO(logger, "Incoming request A: %s", request->data ? "true" : "false"); +} + +void CallbackB(const std::shared_ptr request, + std::shared_ptr response) +{ + response->success = request->data; + auto logger = rclcpp::get_logger("rclcpp"); + RCLCPP_INFO(logger, "Incoming request B: %s", request->data ? "true" : "false"); +} + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + + std::shared_ptr node = rclcpp::Node::make_shared("add_two_ints_server"); + + auto serviceA = + node->create_service("robotA/set_bool", &CallbackA); + auto serviceB = + node->create_service("robotB/set_bool", &CallbackB); + + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Ready."); + + rclcpp::spin(node); + rclcpp::shutdown(); +} diff --git a/btcpp_ros2_samples/src/set_bool_node.cpp b/btcpp_ros2_samples/src/set_bool_node.cpp new file mode 100644 index 0000000..8d8240d --- /dev/null +++ b/btcpp_ros2_samples/src/set_bool_node.cpp @@ -0,0 +1,41 @@ +#include "set_bool_node.hpp" + +bool SetBoolService::setRequest(Request::SharedPtr& request) +{ + getInput("value", request->data); + std::cout << "setRequest " << std::endl; + return true; +} + +BT::NodeStatus SetBoolService::onResponseReceived(const Response::SharedPtr& response) +{ + std::cout << "onResponseReceived " << std::endl; + if(response->success) + { + RCLCPP_INFO(logger(), "SetBool service succeeded."); + return BT::NodeStatus::SUCCESS; + } + else + { + RCLCPP_INFO(logger(), "SetBool service failed: %s", response->message.c_str()); + return BT::NodeStatus::FAILURE; + } +} + +BT::NodeStatus SetBoolService::onFailure(BT::ServiceNodeErrorCode error) +{ + RCLCPP_ERROR(logger(), "Error: %d", error); + return BT::NodeStatus::FAILURE; +} + +//----------------------------------------------------------- + +BT::NodeStatus SetRobotBoolService::tick() +{ + std::string robot; + if(getInput("robot", robot) && !robot.empty()) + { + setServiceName(robot + "/" + service_suffix_); + } + return SetBoolService::tick(); +} diff --git a/btcpp_ros2_samples/src/set_bool_node.hpp b/btcpp_ros2_samples/src/set_bool_node.hpp new file mode 100644 index 0000000..86a12d3 --- /dev/null +++ b/btcpp_ros2_samples/src/set_bool_node.hpp @@ -0,0 +1,51 @@ +#pragma once + +#include +#include "std_srvs/srv/set_bool.hpp" + +using SetBool = std_srvs::srv::SetBool; + +class SetBoolService : public BT::RosServiceNode +{ +public: + explicit SetBoolService(const std::string& name, const BT::NodeConfig& conf, + const BT::RosNodeParams& params) + : RosServiceNode(name, conf, params) + {} + + static BT::PortsList providedPorts() + { + return providedBasicPorts({ BT::InputPort("value") }); + } + + bool setRequest(Request::SharedPtr& request) override; + + BT::NodeStatus onResponseReceived(const Response::SharedPtr& response) override; + + virtual BT::NodeStatus onFailure(BT::ServiceNodeErrorCode error) override; + +private: + std::string service_suffix_; +}; + +//---------------------------------------------- + +class SetRobotBoolService : public SetBoolService +{ +public: + explicit SetRobotBoolService(const std::string& name, const BT::NodeConfig& conf, + const rclcpp::Node::SharedPtr& node, + const std::string& port_name) + : SetBoolService(name, conf, BT::RosNodeParams(node)), service_suffix_(port_name) + {} + + static BT::PortsList providedPorts() + { + return { BT::InputPort("robot"), BT::InputPort("value") }; + } + + BT::NodeStatus tick() override; + +private: + std::string service_suffix_; +};