diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp index a04b1263f4..53ae496d30 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp @@ -58,12 +58,17 @@ class IsPathValidCondition : public BT::ConditionNode { return { BT::InputPort("path", "Path to Check"), + BT::InputPort( + "service_name", + "is_path_valid", + "Name of the service to call, defaults to is_path_valid"), BT::InputPort("server_timeout") }; } private: rclcpp::Node::SharedPtr node_; + std::string service_name_; rclcpp::Client::SharedPtr client_; // The timeout value while waiting for a responce from the // is path valid service diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 360a3bea60..c6211d10e9 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -258,6 +258,7 @@ Path to validate + Name of the service to call, defaults to is_path_valid Server timeout diff --git a/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp b/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp index 4a02dede9a..9e063a3330 100644 --- a/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp @@ -26,10 +26,12 @@ IsPathValidCondition::IsPathValidCondition( : BT::ConditionNode(condition_name, conf) { node_ = config().blackboard->get("node"); - client_ = node_->create_client("is_path_valid"); server_timeout_ = config().blackboard->template get("server_timeout"); getInput("server_timeout", server_timeout_); + + getInput("service_name", service_name_); + client_ = node_->create_client(service_name_); } BT::NodeStatus IsPathValidCondition::tick()