diff --git a/include/behaviortree_ros/bt_action_node.h b/include/behaviortree_ros/bt_action_node.h index a8c21bb..fc4a52a 100644 --- a/include/behaviortree_ros/bt_action_node.h +++ b/include/behaviortree_ros/bt_action_node.h @@ -169,10 +169,22 @@ 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) { - return std::make_unique(node_handle, name, config ); + 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()) { + return std::make_unique(node_handle, name, config ); + } + + 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 ); + } + + throw std::runtime_error("server_name not given as port or default in RegisterRosAction"); }; TreeNodeManifest manifest; 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 7b5070d..5770e85 100644 --- a/test/test_bt.cpp +++ b/test/test_bt.cpp @@ -161,15 +161,13 @@ RosActionNode(handle, name, conf) {} - - + @@ -186,8 +184,8 @@ int main(int argc, char **argv) BehaviorTreeFactory factory; factory.registerNodeType("PrintValue"); - RegisterRosService(factory, "AddTwoInts", nh); - RegisterRosAction(factory, "Fibonacci", nh); + RegisterRosService(factory, "AddTwoInts", nh, "add_two_ints"); + RegisterRosAction(factory, "Fibonacci", nh, "fibonacci"); auto tree = factory.createTreeFromText(xml_text);