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..5acc938a28f 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,11 @@ class IsPathValidCondition : public BT::ConditionNode { return { BT::InputPort("path", "Path to Check"), - BT::InputPort("server_timeout") + BT::InputPort("server_timeout"), + BT::InputPort("max_cost", 253, "Maximum cost of the path"), + BT::InputPort( + "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 diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 0afe09c4bdd..b8275a96a85 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -107,6 +107,7 @@ A vector of goals to check if in collision Whether to use the footprint cost or the point cost. The cost threshold above which a waypoint is considered in collision and should be removed. + If unknown cost is considered valid A vector of goals containing only those that are not in collision. @@ -283,6 +284,8 @@ Path to validate Server timeout + Maximum cost of the path + If unknown cost is considered valid 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..8caed0c3971 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_(253), consider_unknown_as_obstacle_(false) { node_ = config().blackboard->get("node"); client_ = node_->create_client("is_path_valid"); @@ -34,6 +35,8 @@ IsPathValidCondition::IsPathValidCondition( void IsPathValidCondition::initialize() { getInput("server_timeout", server_timeout_); + getInput("max_cost", max_cost_); + getInput("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(); 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_) == diff --git a/nav2_msgs/srv/IsPathValid.srv b/nav2_msgs/srv/IsPathValid.srv index 8d6dc3b38dd..e178847085a 100644 --- a/nav2_msgs/srv/IsPathValid.srv +++ b/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 diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index c227851f07b..03cf80fdf1c 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/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; } diff --git a/nav2_system_tests/src/planning/planner_tester.cpp b/nav2_system_tests/src/planning/planner_tester.cpp index 0a2368bab6b..eda7ce0b853 100644 --- a/nav2_system_tests/src/planning/planner_tester.cpp +++ b/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(); 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"); diff --git a/nav2_system_tests/src/planning/planner_tester.hpp b/nav2_system_tests/src/planning/planner_tester.hpp index 03081726000..c7e450cfe6f 100644 --- a/nav2_system_tests/src/planning/planner_tester.hpp +++ b/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(); 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..59f1ac5a386 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,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)