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 49143581ece..8ba89879264 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 @@ -63,7 +63,8 @@ class IsPathValidCondition : public BT::ConditionNode { return { BT::InputPort("path", "Path to Check"), - BT::InputPort("server_timeout") + BT::InputPort("server_timeout"), + BT::InputPort("max_cost", 255, "Maximum cost of the path") }; } @@ -73,6 +74,7 @@ class IsPathValidCondition : public BT::ConditionNode // The timeout value while waiting for a response from the // is path valid service std::chrono::milliseconds server_timeout_; + unsigned int max_cost_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 0afe09c4bdd..785524b0085 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -283,6 +283,7 @@ Path to validate Server timeout + Maximum cost of the path 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 cd76df97495..a72648cb2e0 100644 --- a/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp @@ -23,7 +23,8 @@ namespace nav2_behavior_tree IsPathValidCondition::IsPathValidCondition( const std::string & condition_name, const BT::NodeConfiguration & conf) -: BT::ConditionNode(condition_name, conf) +: BT::ConditionNode(condition_name, conf), + max_cost_(255) { node_ = config().blackboard->get("node"); client_ = node_->create_client("is_path_valid"); @@ -34,6 +35,7 @@ IsPathValidCondition::IsPathValidCondition( void IsPathValidCondition::initialize() { getInput("server_timeout", server_timeout_); + getInput("max_cost", max_cost_); } BT::NodeStatus IsPathValidCondition::tick() @@ -48,6 +50,7 @@ BT::NodeStatus IsPathValidCondition::tick() auto request = std::make_shared(); request->path = path; + request->max_cost = max_cost_; auto result = client_->async_send_request(request); if (rclcpp::spin_until_future_complete(node_, result, server_timeout_) == diff --git a/nav2_msgs/srv/IsPathValid.srv b/nav2_msgs/srv/IsPathValid.srv index 8d6dc3b38dd..ae9e5eaf0fd 100644 --- a/nav2_msgs/srv/IsPathValid.srv +++ b/nav2_msgs/srv/IsPathValid.srv @@ -1,6 +1,7 @@ #Determine if the current path is still valid nav_msgs/Path path +uint8 max_cost 254 --- bool is_valid int32[] invalid_pose_indices diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index c227851f07b..6a827cf07a3 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -700,12 +700,12 @@ void PlannerServer::isPathValid( } if (use_radius && - (cost == nav2_costmap_2d::LETHAL_OBSTACLE || + (cost > request->max_cost || cost == nav2_costmap_2d::LETHAL_OBSTACLE || cost == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE)) { response->is_valid = false; break; - } else if (cost == nav2_costmap_2d::LETHAL_OBSTACLE) { + } else if (cost == nav2_costmap_2d::LETHAL_OBSTACLE || cost > request->max_cost) { response->is_valid = false; break; } diff --git a/nav2_system_tests/src/planning/planner_tester.cpp b/nav2_system_tests/src/planning/planner_tester.cpp index 0a2368bab6b..4cfb0f32af4 100644 --- a/nav2_system_tests/src/planning/planner_tester.cpp +++ b/nav2_system_tests/src/planning/planner_tester.cpp @@ -397,12 +397,13 @@ TaskStatus PlannerTester::createPlan( return TaskStatus::FAILED; } -bool PlannerTester::isPathValid(nav_msgs::msg::Path & path) +bool PlannerTester::isPathValid(nav_msgs::msg::Path & path, unsigned int max_cost) { planner_tester_->setCostmap(costmap_.get()); // create a fake service request auto request = std::make_shared(); request->path = path; + request->max_cost = max_cost; auto result = path_valid_client_->async_send_request(request); RCLCPP_INFO(this->get_logger(), "Waiting for service complete"); diff --git a/nav2_system_tests/src/planning/planner_tester.hpp b/nav2_system_tests/src/planning/planner_tester.hpp index 03081726000..16264909ee2 100644 --- a/nav2_system_tests/src/planning/planner_tester.hpp +++ b/nav2_system_tests/src/planning/planner_tester.hpp @@ -158,7 +158,7 @@ class PlannerTester : public rclcpp::Node const unsigned int number_tests, const float acceptable_fail_ratio); - bool isPathValid(nav_msgs::msg::Path & path); + bool isPathValid(nav_msgs::msg::Path & path, unsigned int max_cost); private: void setCostmap(); diff --git a/nav2_system_tests/src/planning/test_planner_is_path_valid.cpp b/nav2_system_tests/src/planning/test_planner_is_path_valid.cpp index cf4b7c90a8a..68c38ca28bf 100644 --- a/nav2_system_tests/src/planning/test_planner_is_path_valid.cpp +++ b/nav2_system_tests/src/planning/test_planner_is_path_valid.cpp @@ -32,9 +32,10 @@ TEST(testIsPathValid, testIsPathValid) planner_tester->loadSimpleCostmap(TestCostmap::top_left_obstacle); nav_msgs::msg::Path path; + unsigned int max_cost = 255; // empty path - bool is_path_valid = planner_tester->isPathValid(path); + bool is_path_valid = planner_tester->isPathValid(path, max_cost); EXPECT_FALSE(is_path_valid); // invalid path @@ -46,7 +47,7 @@ TEST(testIsPathValid, testIsPathValid) path.poses.push_back(pose); } } - is_path_valid = planner_tester->isPathValid(path); + is_path_valid = planner_tester->isPathValid(path, max_cost); EXPECT_FALSE(is_path_valid); // valid path @@ -57,8 +58,13 @@ TEST(testIsPathValid, testIsPathValid) pose.pose.position.y = i; path.poses.push_back(pose); } - is_path_valid = planner_tester->isPathValid(path); + is_path_valid = planner_tester->isPathValid(path, max_cost); EXPECT_TRUE(is_path_valid); + + // valid path but higher than max cost + max_cost = 0; + is_path_valid = planner_tester->isPathValid(path, max_cost); + EXPECT_FALSE(is_path_valid); } int main(int argc, char ** argv)