Add ispathvalid maxcost (#4873)

* Add ispathvalid maxcost

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

* Fix problems as requested

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

* Set default as 253, Add considered unknown as obstacle

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

* Edit comment

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

* Update nav2_planner/src/planner_server.cpp

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

---------

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>
Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

Author: mini-1235

nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp

@@ -63,7 +63,11 @@ 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<unsigned int>("max_cost", 253, "Maximum cost of the path"),
+      BT::InputPort<bool>(
+          "consider_unknown_as_obstacle", false,
+          "Whether to consider unknown cost as obstacle")
     };
   }
 
@@ -73,6 +77,8 @@ 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_;
+  bool consider_unknown_as_obstacle_;
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/nav2_tree_nodes.xml

@@ -107,6 +107,7 @@
       <input_port name="input_goals">A vector of goals to check if in collision</input_port>
       <input_port name="use_footprint">Whether to use the footprint cost or the point cost.</input_port>
       <input_port name="cost_threshold">The cost threshold above which a waypoint is considered in collision and should be removed.</input_port>
+      <input_port name="consider_unknown_as_obstacle"> If unknown cost is considered valid </input_port>
       <output_port name="output_goals">A vector of goals containing only those that are not in collision.</output_port>
     </Action>
 
@@ -283,6 +284,8 @@
     <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>
+      <input_port name="consider_unknown_as_obstacle"> If unknown cost is considered valid </input_port>
     </Condition>
 
     <Condition ID="WouldAControllerRecoveryHelp">

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_(253), consider_unknown_as_obstacle_(false)
 {
   node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
   client_ = node_->create_client<nav2_msgs::srv::IsPathValid>("is_path_valid");
@@ -34,6 +35,8 @@ IsPathValidCondition::IsPathValidCondition(
 void IsPathValidCondition::initialize()
 {
   getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);
+  getInput<unsigned int>("max_cost", max_cost_);
+  getInput<bool>("consider_unknown_as_obstacle", consider_unknown_as_obstacle_);
 }
 
 BT::NodeStatus IsPathValidCondition::tick()
@@ -48,6 +51,8 @@ BT::NodeStatus IsPathValidCondition::tick()
   auto request = std::make_shared<nav2_msgs::srv::IsPathValid::Request>();
 
   request->path = path;
+  request->max_cost = max_cost_;
+  request->consider_unknown_as_obstacle = consider_unknown_as_obstacle_;
   auto result = client_->async_send_request(request);
 
   if (rclcpp::spin_until_future_complete(node_, result, server_timeout_) ==

nav2_msgs/srv/IsPathValid.srv

@@ -1,6 +1,8 @@
 #Determine if the current path is still valid
 
 nav_msgs/Path path
+uint8 max_cost 253
+bool consider_unknown_as_obstacle false
 ---
 bool is_valid
 int32[] invalid_pose_indices 

nav2_planner/src/planner_server.cpp

@@ -699,13 +699,19 @@ void PlannerServer::isPathValid(
             position.x, position.y, theta, footprint));
       }
 
+      if (cost == nav2_costmap_2d::NO_INFORMATION && request->consider_unknown_as_obstacle) {
+        cost = nav2_costmap_2d::LETHAL_OBSTACLE;
+      } else if (cost == nav2_costmap_2d::NO_INFORMATION) {
+        cost = nav2_costmap_2d::FREE_SPACE;
+      }
+
       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;
       }

nav2_system_tests/src/planning/planner_tester.cpp

@@ -397,12 +397,16 @@ 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,
+  bool consider_unknown_as_obstacle)
 {
   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;
+  request->consider_unknown_as_obstacle = consider_unknown_as_obstacle;
   auto result = path_valid_client_->async_send_request(request);
 
   RCLCPP_INFO(this->get_logger(), "Waiting for service complete");

nav2_system_tests/src/planning/planner_tester.hpp

@@ -158,7 +158,9 @@ 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,
+    bool consider_unknown_as_obstacle);
 
 private:
   void setCostmap();

nav2_system_tests/src/planning/test_planner_is_path_valid.cpp

@@ -32,9 +32,11 @@ TEST(testIsPathValid, testIsPathValid)
   planner_tester->loadSimpleCostmap(TestCostmap::top_left_obstacle);
 
   nav_msgs::msg::Path path;
+  unsigned int max_cost = 253;
+  bool consider_unknown_as_obstacle = false;
 
   // empty path
-  bool is_path_valid = planner_tester->isPathValid(path);
+  bool is_path_valid = planner_tester->isPathValid(path, max_cost, consider_unknown_as_obstacle);
   EXPECT_FALSE(is_path_valid);
 
   // invalid path
@@ -46,7 +48,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, consider_unknown_as_obstacle);
   EXPECT_FALSE(is_path_valid);
 
   // valid path
@@ -57,8 +59,25 @@ 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, consider_unknown_as_obstacle);
   EXPECT_TRUE(is_path_valid);
+
+  // valid path, but contains NO_INFORMATION(255)
+  path.poses.clear();
+  consider_unknown_as_obstacle = true;
+  for (float i = 0; i < 10; i += 1.0) {
+    geometry_msgs::msg::PoseStamped pose;
+    pose.pose.position.x = 1.0;
+    pose.pose.position.y = i;
+    path.poses.push_back(pose);
+  }
+  is_path_valid = planner_tester->isPathValid(path, max_cost, consider_unknown_as_obstacle);
+  EXPECT_FALSE(is_path_valid);
+
+  // valid path but higher than max cost
+  max_cost = 0;
+  is_path_valid = planner_tester->isPathValid(path, max_cost, consider_unknown_as_obstacle);
+  EXPECT_FALSE(is_path_valid);
 }
 
 int main(int argc, char ** argv)