Add ispathvalid maxcost

Signed-off-by: mini-1235 <[email protected]>
mini-1235 · Jan 25, 2025 · 969c34c · 969c34c
1 parent 6e1c85e
commit 969c34c
Show file tree

Hide file tree

Showing 8 changed files with 22 additions and 8 deletions.
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
@@ -63,7 +63,8 @@ class IsPathValidCondition : public BT::ConditionNode
   {
     return {
       BT::InputPort<nav_msgs::msg::Path>("path", "Path to Check"),
-      BT::InputPort<std::chrono::milliseconds>("server_timeout")
+      BT::InputPort<std::chrono::milliseconds>("server_timeout"),
+      BT::InputPort<uint8_t>("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_;
+  uint8_t max_cost_;
 };
 
 }  // namespace nav2_behavior_tree

diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml
@@ -283,6 +283,7 @@
     <Condition ID="IsPathValid">
       <input_port name="path"> Path to validate </input_port>
       <input_port name="server_timeout"> Server timeout </input_port>
+      <input_port name="max_cost"> Maximum cost of the path </input_port>
     </Condition>
 
     <Condition ID="WouldAControllerRecoveryHelp">

diff --git 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<rclcpp::Node::SharedPtr>("node");
   client_ = node_->create_client<nav2_msgs::srv::IsPathValid>("is_path_valid");
@@ -34,6 +35,7 @@ IsPathValidCondition::IsPathValidCondition(
 void IsPathValidCondition::initialize()
 {
   getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);
+  getInput<uint8_t>("max_cost", max_cost_);
 }
 
 BT::NodeStatus IsPathValidCondition::tick()
@@ -48,6 +50,7 @@ BT::NodeStatus IsPathValidCondition::tick()
   auto request = std::make_shared<nav2_msgs::srv::IsPathValid::Request>();
 
   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
@@ -1,6 +1,7 @@
 #Determine if the current path is still valid
 
 nav_msgs/Path path
+uint8 max_cost
 ---
 bool is_valid
 int32[] invalid_pose_indices 
diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp
@@ -705,7 +705,7 @@ void PlannerServer::isPathValid(
       {
         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
@@ -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, uint8_t max_cost)
 {
   planner_tester_->setCostmap(costmap_.get());
   // create a fake service request
   auto request = std::make_shared<nav2_msgs::srv::IsPathValid::Request>();
   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
@@ -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, uint8_t 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
@@ -32,9 +32,10 @@ TEST(testIsPathValid, testIsPathValid)
   planner_tester->loadSimpleCostmap(TestCostmap::top_left_obstacle);
 
   nav_msgs::msg::Path path;
+  uint8_t 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)