From b8858659321916e28f33d3d91f5fd9679b04d676 Mon Sep 17 00:00:00 2001 From: Oskar Date: Fri, 20 May 2022 11:12:39 +0200 Subject: [PATCH 1/7] Add to Action constructor --- include/behaviortree_ros/bt_action_node.h | 10 +++++----- test/test_bt.cpp | 4 ++-- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/include/behaviortree_ros/bt_action_node.h b/include/behaviortree_ros/bt_action_node.h index a8c21bb..cfba48c 100644 --- a/include/behaviortree_ros/bt_action_node.h +++ b/include/behaviortree_ros/bt_action_node.h @@ -40,11 +40,11 @@ class RosActionNode : public BT::ActionNodeBase { protected: - RosActionNode(ros::NodeHandle& nh, const std::string& name, const BT::NodeConfiguration & conf): + RosActionNode(ros::NodeHandle& nh, const std::string& name, const BT::NodeConfiguration & conf, const std::string& server_name): BT::ActionNodeBase(name, conf), node_(nh) { - const std::string server_name = getInput("server_name").value(); - action_client_ = std::make_shared( node_, server_name, true ); + const std::string server_name_ = server_name.empty() ? getInput("server_name").value() : server_name; + action_client_ = std::make_shared( node_, server_name_, true ); } public: @@ -172,7 +172,7 @@ template static ros::NodeHandle& node_handle) { NodeBuilder builder = [&node_handle](const std::string& name, const NodeConfiguration& config) { - return std::make_unique(node_handle, name, config ); + return std::make_unique(node_handle, name, config, "" ); }; TreeNodeManifest manifest; @@ -187,4 +187,4 @@ template static } // namespace BT -#endif // BEHAVIOR_TREE_BT_ACTION_NODE_HPP_ +#endif // BEHAVIOR_TREE_BT_ACTION_NODE_HPP_ \ No newline at end of file diff --git a/test/test_bt.cpp b/test/test_bt.cpp index 7b5070d..fe30683 100644 --- a/test/test_bt.cpp +++ b/test/test_bt.cpp @@ -94,8 +94,8 @@ class FibonacciServer: public RosActionNode { public: - FibonacciServer( ros::NodeHandle& handle, const std::string& name, const NodeConfiguration & conf): -RosActionNode(handle, name, conf) {} + FibonacciServer( ros::NodeHandle& handle, const std::string& name, const NodeConfiguration & conf, const std::string& server_name): +RosActionNode(handle, name, conf, server_name) {} static PortsList providedPorts() { From 8ebdfbebd8e639b6b606a194a51cc8aea7aac59c Mon Sep 17 00:00:00 2001 From: Oskar Date: Fri, 20 May 2022 11:15:43 +0200 Subject: [PATCH 2/7] Add to RegisterRosAction --- include/behaviortree_ros/bt_action_node.h | 7 ++++--- test/test_bt.cpp | 2 +- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/include/behaviortree_ros/bt_action_node.h b/include/behaviortree_ros/bt_action_node.h index cfba48c..9e252c5 100644 --- a/include/behaviortree_ros/bt_action_node.h +++ b/include/behaviortree_ros/bt_action_node.h @@ -169,10 +169,11 @@ class RosActionNode : public BT::ActionNodeBase template static void RegisterRosAction(BT::BehaviorTreeFactory& factory, const std::string& registration_ID, - ros::NodeHandle& node_handle) + ros::NodeHandle& node_handle, + const std::string& server_name) { - NodeBuilder builder = [&node_handle](const std::string& name, const NodeConfiguration& config) { - return std::make_unique(node_handle, name, config, "" ); + NodeBuilder builder = [&node_handle, &server_name](const std::string& name, const NodeConfiguration& config) { + return std::make_unique(node_handle, name, config, server_name ); }; TreeNodeManifest manifest; diff --git a/test/test_bt.cpp b/test/test_bt.cpp index fe30683..9eb19b3 100644 --- a/test/test_bt.cpp +++ b/test/test_bt.cpp @@ -187,7 +187,7 @@ int main(int argc, char **argv) factory.registerNodeType("PrintValue"); RegisterRosService(factory, "AddTwoInts", nh); - RegisterRosAction(factory, "Fibonacci", nh); + RegisterRosAction(factory, "Fibonacci", nh, ""); auto tree = factory.createTreeFromText(xml_text); From 5e87f1dfd656532126ef17f4fd77348681b4b825 Mon Sep 17 00:00:00 2001 From: Oskar Date: Fri, 20 May 2022 11:36:13 +0200 Subject: [PATCH 3/7] Use parameter overloading - leads to more duplication in RegisterRosAction --- include/behaviortree_ros/bt_action_node.h | 30 ++++++++++++++++++++--- test/test_bt.cpp | 4 +-- 2 files changed, 29 insertions(+), 5 deletions(-) diff --git a/include/behaviortree_ros/bt_action_node.h b/include/behaviortree_ros/bt_action_node.h index 9e252c5..e12d641 100644 --- a/include/behaviortree_ros/bt_action_node.h +++ b/include/behaviortree_ros/bt_action_node.h @@ -40,11 +40,17 @@ class RosActionNode : public BT::ActionNodeBase { protected: + RosActionNode(ros::NodeHandle& nh, const std::string& name, const BT::NodeConfiguration & conf): + BT::ActionNodeBase(name, conf), node_(nh) + { + const std::string server_name = getInput("server_name").value(); + action_client_ = std::make_shared( node_, server_name, true ); + } + RosActionNode(ros::NodeHandle& nh, const std::string& name, const BT::NodeConfiguration & conf, const std::string& server_name): BT::ActionNodeBase(name, conf), node_(nh) { - const std::string server_name_ = server_name.empty() ? getInput("server_name").value() : server_name; - action_client_ = std::make_shared( node_, server_name_, true ); + action_client_ = std::make_shared( node_, server_name, true ); } public: @@ -64,7 +70,7 @@ class RosActionNode : public BT::ActionNodeBase static PortsList providedPorts() { return { - InputPort("server_name", "name of the Action Server"), + InputPort("server_name", "", "name of the Action Server"), InputPort("timeout", 500, "timeout to connect (milliseconds)") }; } @@ -166,6 +172,24 @@ class RosActionNode : public BT::ActionNodeBase /// Method to register the service into a factory. /// It gives you the opportunity to set the ros::NodeHandle. +template static + void RegisterRosAction(BT::BehaviorTreeFactory& factory, + const std::string& registration_ID, + ros::NodeHandle& node_handle) +{ + NodeBuilder builder = [&node_handle](const std::string& name, const NodeConfiguration& config) { + return std::make_unique(node_handle, name, config ); + }; + + TreeNodeManifest manifest; + manifest.type = getType(); + manifest.ports = DerivedT::providedPorts(); + manifest.registration_ID = registration_ID; + const auto& basic_ports = RosActionNode< typename DerivedT::ActionType>::providedPorts(); + manifest.ports.insert( basic_ports.begin(), basic_ports.end() ); + factory.registerBuilder( manifest, builder ); +} + template static void RegisterRosAction(BT::BehaviorTreeFactory& factory, const std::string& registration_ID, diff --git a/test/test_bt.cpp b/test/test_bt.cpp index 9eb19b3..423f042 100644 --- a/test/test_bt.cpp +++ b/test/test_bt.cpp @@ -95,7 +95,7 @@ class FibonacciServer: public RosActionNode public: FibonacciServer( ros::NodeHandle& handle, const std::string& name, const NodeConfiguration & conf, const std::string& server_name): -RosActionNode(handle, name, conf, server_name) {} + RosActionNode(handle, name, conf, server_name) {} static PortsList providedPorts() { @@ -187,7 +187,7 @@ int main(int argc, char **argv) factory.registerNodeType("PrintValue"); RegisterRosService(factory, "AddTwoInts", nh); - RegisterRosAction(factory, "Fibonacci", nh, ""); + RegisterRosAction(factory, "Fibonacci", nh, "fibonacci"); auto tree = factory.createTreeFromText(xml_text); From c95a38798c2d165d79b0764d8bb85c564749a522 Mon Sep 17 00:00:00 2001 From: Oskar Date: Fri, 20 May 2022 13:07:59 +0200 Subject: [PATCH 4/7] Revert and add new implementation in RegisterRosAction --- include/behaviortree_ros/bt_action_node.h | 44 +++++++++-------------- test/test_bt.cpp | 7 ++-- 2 files changed, 19 insertions(+), 32 deletions(-) diff --git a/include/behaviortree_ros/bt_action_node.h b/include/behaviortree_ros/bt_action_node.h index e12d641..de0ad4e 100644 --- a/include/behaviortree_ros/bt_action_node.h +++ b/include/behaviortree_ros/bt_action_node.h @@ -47,12 +47,6 @@ class RosActionNode : public BT::ActionNodeBase action_client_ = std::make_shared( node_, server_name, true ); } - RosActionNode(ros::NodeHandle& nh, const std::string& name, const BT::NodeConfiguration & conf, const std::string& server_name): - BT::ActionNodeBase(name, conf), node_(nh) - { - action_client_ = std::make_shared( node_, server_name, true ); - } - public: using BaseClass = RosActionNode; @@ -70,7 +64,7 @@ class RosActionNode : public BT::ActionNodeBase static PortsList providedPorts() { return { - InputPort("server_name", "", "name of the Action Server"), + InputPort("server_name", "name of the Action Server"), InputPort("timeout", 500, "timeout to connect (milliseconds)") }; } @@ -175,29 +169,23 @@ class RosActionNode : public BT::ActionNodeBase template static void RegisterRosAction(BT::BehaviorTreeFactory& factory, const std::string& registration_ID, - ros::NodeHandle& node_handle) + ros::NodeHandle& node_handle, + const std::string& default_server_name = {} ) { - NodeBuilder builder = [&node_handle](const std::string& name, const NodeConfiguration& config) { + NodeBuilder builder = [&node_handle, default_server_name](const std::string& name, const NodeConfiguration& config) { return std::make_unique(node_handle, name, config ); - }; - - TreeNodeManifest manifest; - manifest.type = getType(); - manifest.ports = DerivedT::providedPorts(); - manifest.registration_ID = registration_ID; - const auto& basic_ports = RosActionNode< typename DerivedT::ActionType>::providedPorts(); - manifest.ports.insert( basic_ports.begin(), basic_ports.end() ); - factory.registerBuilder( manifest, builder ); -} -template static - void RegisterRosAction(BT::BehaviorTreeFactory& factory, - const std::string& registration_ID, - ros::NodeHandle& node_handle, - const std::string& server_name) -{ - NodeBuilder builder = [&node_handle, &server_name](const std::string& name, const NodeConfiguration& config) { - return std::make_unique(node_handle, name, config, server_name ); + auto name_it = config.input_ports.find("server_name"); + // The server_name passed as port has priority over the default_server_name + if( !default_server_name.empty() && name_it->second.empty() ) + { + auto new_config = config; + new_config.input_ports["server_name"] = default_server_name; + return std::make_unique(node_handle, name, new_config ); + } + else{ + return std::make_unique(node_handle, name, config ); + } }; TreeNodeManifest manifest; @@ -212,4 +200,4 @@ template static } // namespace BT -#endif // BEHAVIOR_TREE_BT_ACTION_NODE_HPP_ \ No newline at end of file +#endif // BEHAVIOR_TREE_BT_ACTION_NODE_HPP_ diff --git a/test/test_bt.cpp b/test/test_bt.cpp index 423f042..dc60ef5 100644 --- a/test/test_bt.cpp +++ b/test/test_bt.cpp @@ -94,8 +94,8 @@ class FibonacciServer: public RosActionNode { public: - FibonacciServer( ros::NodeHandle& handle, const std::string& name, const NodeConfiguration & conf, const std::string& server_name): - RosActionNode(handle, name, conf, server_name) {} + FibonacciServer( ros::NodeHandle& handle, const std::string& name, const NodeConfiguration & conf): +RosActionNode(handle, name, conf) {} static PortsList providedPorts() { @@ -168,8 +168,7 @@ class FibonacciServer: public RosActionNode - + From 6eff47f5f32a937a2983f34dbe270517ac8a56bd Mon Sep 17 00:00:00 2001 From: = <=> Date: Tue, 24 May 2022 10:41:38 +0200 Subject: [PATCH 5/7] First working version --- include/behaviortree_ros/bt_action_node.h | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/include/behaviortree_ros/bt_action_node.h b/include/behaviortree_ros/bt_action_node.h index de0ad4e..fee8d3d 100644 --- a/include/behaviortree_ros/bt_action_node.h +++ b/include/behaviortree_ros/bt_action_node.h @@ -173,19 +173,22 @@ template static const std::string& default_server_name = {} ) { NodeBuilder builder = [&node_handle, default_server_name](const std::string& name, const NodeConfiguration& config) { - return std::make_unique(node_handle, name, config ); + + auto server_name_port = config.input_ports.find("server_name"); + if ((server_name_port == config.input_ports.end() || server_name_port->second.empty()) && default_server_name.empty()) + { + throw std::runtime_error("server_name not given as port or default in RegisterRosAction"); + } - auto name_it = config.input_ports.find("server_name"); // The server_name passed as port has priority over the default_server_name - if( !default_server_name.empty() && name_it->second.empty() ) + if( server_name_port == config.input_ports.end() || server_name_port->second.empty() ) { auto new_config = config; new_config.input_ports["server_name"] = default_server_name; return std::make_unique(node_handle, name, new_config ); } - else{ - return std::make_unique(node_handle, name, config ); - } + + return std::make_unique(node_handle, name, config ); }; TreeNodeManifest manifest; From 9feac427f760d3555d908e1ae17bfa5f91a6afd7 Mon Sep 17 00:00:00 2001 From: = <=> Date: Tue, 24 May 2022 10:45:27 +0200 Subject: [PATCH 6/7] Refactored version of bt_action_node --- include/behaviortree_ros/bt_action_node.h | 14 +++++--------- 1 file changed, 5 insertions(+), 9 deletions(-) diff --git a/include/behaviortree_ros/bt_action_node.h b/include/behaviortree_ros/bt_action_node.h index fee8d3d..fc4a52a 100644 --- a/include/behaviortree_ros/bt_action_node.h +++ b/include/behaviortree_ros/bt_action_node.h @@ -173,22 +173,18 @@ template static const std::string& default_server_name = {} ) { NodeBuilder builder = [&node_handle, default_server_name](const std::string& name, const NodeConfiguration& config) { - auto server_name_port = config.input_ports.find("server_name"); - if ((server_name_port == config.input_ports.end() || server_name_port->second.empty()) && default_server_name.empty()) - { - throw std::runtime_error("server_name not given as port or default in RegisterRosAction"); + if (server_name_port != config.input_ports.end() && !server_name_port->second.empty()) { + return std::make_unique(node_handle, name, config ); } - // The server_name passed as port has priority over the default_server_name - if( server_name_port == config.input_ports.end() || server_name_port->second.empty() ) - { + if (!default_server_name.empty()) { auto new_config = config; new_config.input_ports["server_name"] = default_server_name; return std::make_unique(node_handle, name, new_config ); } - - return std::make_unique(node_handle, name, config ); + + throw std::runtime_error("server_name not given as port or default in RegisterRosAction"); }; TreeNodeManifest manifest; From 01dd884bb751fb4a42d24ab429f40c26afb7d36a Mon Sep 17 00:00:00 2001 From: = <=> Date: Tue, 24 May 2022 10:50:23 +0200 Subject: [PATCH 7/7] Add default_service_name to bt_service_node --- include/behaviortree_ros/bt_service_node.h | 18 +++++++++++++++--- test/test_bt.cpp | 5 ++--- 2 files changed, 17 insertions(+), 6 deletions(-) diff --git a/include/behaviortree_ros/bt_service_node.h b/include/behaviortree_ros/bt_service_node.h index a60eff6..64d5675 100644 --- a/include/behaviortree_ros/bt_service_node.h +++ b/include/behaviortree_ros/bt_service_node.h @@ -116,10 +116,22 @@ class RosServiceNode : public BT::SyncActionNode template static void RegisterRosService(BT::BehaviorTreeFactory& factory, const std::string& registration_ID, - ros::NodeHandle& node_handle) + ros::NodeHandle& node_handle, + const std::string& default_service_name = {} ) { - NodeBuilder builder = [&node_handle](const std::string& name, const NodeConfiguration& config) { - return std::make_unique(node_handle, name, config ); + NodeBuilder builder = [&node_handle, default_service_name](const std::string& name, const NodeConfiguration& config) { + auto service_name_port = config.input_ports.find("service_name"); + if (service_name_port != config.input_ports.end() && !service_name_port->second.empty()) { + return std::make_unique(node_handle, name, config ); + } + + if (!default_service_name.empty()) { + auto new_config = config; + new_config.input_ports["service_name"] = default_service_name; + return std::make_unique(node_handle, name, new_config ); + } + + throw std::runtime_error("service_name not given as port or default in RegisterRosService"); }; TreeNodeManifest manifest; diff --git a/test/test_bt.cpp b/test/test_bt.cpp index dc60ef5..5770e85 100644 --- a/test/test_bt.cpp +++ b/test/test_bt.cpp @@ -161,8 +161,7 @@ RosActionNode(handle, name, conf) {} - @@ -185,7 +184,7 @@ int main(int argc, char **argv) BehaviorTreeFactory factory; factory.registerNodeType("PrintValue"); - RegisterRosService(factory, "AddTwoInts", nh); + RegisterRosService(factory, "AddTwoInts", nh, "add_two_ints"); RegisterRosAction(factory, "Fibonacci", nh, "fibonacci"); auto tree = factory.createTreeFromText(xml_text);