From 969c34cb72e4de51e799a8e951ffeb10d716f572 Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Sat, 25 Jan 2025 08:03:42 +0000 Subject: [PATCH 1/5] Add ispathvalid maxcost Signed-off-by: mini-1235 --- .../plugins/condition/is_path_valid_condition.hpp | 4 +++- nav2_behavior_tree/nav2_tree_nodes.xml | 1 + .../plugins/condition/is_path_valid_condition.cpp | 5 ++++- nav2_msgs/srv/IsPathValid.srv | 1 + nav2_planner/src/planner_server.cpp | 2 +- nav2_system_tests/src/planning/planner_tester.cpp | 3 ++- nav2_system_tests/src/planning/planner_tester.hpp | 2 +- .../src/planning/test_planner_is_path_valid.cpp | 12 +++++++++--- 8 files changed, 22 insertions(+), 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 index 49143581ece..61432d71c80 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_; + 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 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..6525dd5225e 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..407f1ce9f22 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 --- 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..d979bae7739 100644 --- 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 index 0a2368bab6b..69524bd463c 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, uint8_t 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..262bb32b2b3 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, 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 index cf4b7c90a8a..8daa2503d4a 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; + 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) From 4a81119e59d0bc4c5b469c5a49ce351bb65e64cf Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Tue, 28 Jan 2025 16:13:49 +0000 Subject: [PATCH 2/5] Fix problems as requested Signed-off-by: mini-1235 --- .../plugins/condition/is_path_valid_condition.hpp | 4 ++-- .../plugins/condition/is_path_valid_condition.cpp | 2 +- nav2_msgs/srv/IsPathValid.srv | 2 +- nav2_planner/src/planner_server.cpp | 2 +- nav2_system_tests/src/planning/planner_tester.cpp | 2 +- nav2_system_tests/src/planning/planner_tester.hpp | 2 +- nav2_system_tests/src/planning/test_planner_is_path_valid.cpp | 2 +- 7 files changed, 8 insertions(+), 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 index 61432d71c80..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 @@ -64,7 +64,7 @@ class IsPathValidCondition : public BT::ConditionNode return { BT::InputPort("path", "Path to Check"), BT::InputPort("server_timeout"), - BT::InputPort("max_cost", 255, "Maximum cost of the path") + BT::InputPort("max_cost", 255, "Maximum cost of the path") }; } @@ -74,7 +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_; + unsigned int max_cost_; }; } // namespace nav2_behavior_tree 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 6525dd5225e..a72648cb2e0 100644 --- a/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp @@ -35,7 +35,7 @@ IsPathValidCondition::IsPathValidCondition( void IsPathValidCondition::initialize() { getInput("server_timeout", server_timeout_); - getInput("max_cost", max_cost_); + getInput("max_cost", max_cost_); } BT::NodeStatus IsPathValidCondition::tick() diff --git a/nav2_msgs/srv/IsPathValid.srv b/nav2_msgs/srv/IsPathValid.srv index 407f1ce9f22..ae9e5eaf0fd 100644 --- a/nav2_msgs/srv/IsPathValid.srv +++ b/nav2_msgs/srv/IsPathValid.srv @@ -1,7 +1,7 @@ #Determine if the current path is still valid nav_msgs/Path path -uint8 max_cost +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 d979bae7739..6a827cf07a3 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -700,7 +700,7 @@ 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; diff --git a/nav2_system_tests/src/planning/planner_tester.cpp b/nav2_system_tests/src/planning/planner_tester.cpp index 69524bd463c..4cfb0f32af4 100644 --- a/nav2_system_tests/src/planning/planner_tester.cpp +++ b/nav2_system_tests/src/planning/planner_tester.cpp @@ -397,7 +397,7 @@ TaskStatus PlannerTester::createPlan( return TaskStatus::FAILED; } -bool PlannerTester::isPathValid(nav_msgs::msg::Path & path, uint8_t max_cost) +bool PlannerTester::isPathValid(nav_msgs::msg::Path & path, unsigned int max_cost) { planner_tester_->setCostmap(costmap_.get()); // create a fake service request diff --git a/nav2_system_tests/src/planning/planner_tester.hpp b/nav2_system_tests/src/planning/planner_tester.hpp index 262bb32b2b3..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, uint8_t max_cost); + 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 8daa2503d4a..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,7 +32,7 @@ TEST(testIsPathValid, testIsPathValid) planner_tester->loadSimpleCostmap(TestCostmap::top_left_obstacle); nav_msgs::msg::Path path; - uint8_t max_cost = 255; + unsigned int max_cost = 255; // empty path bool is_path_valid = planner_tester->isPathValid(path, max_cost); From 342378e5bac47aff56d58c6769c343e1a7d98219 Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Wed, 29 Jan 2025 21:14:15 +0000 Subject: [PATCH 3/5] Set default as 253, Add considered unknown as obstacle Signed-off-by: mini-1235 --- .../condition/is_path_valid_condition.hpp | 6 ++++- nav2_behavior_tree/nav2_tree_nodes.xml | 2 ++ .../condition/is_path_valid_condition.cpp | 4 ++- nav2_msgs/srv/IsPathValid.srv | 3 ++- nav2_planner/src/planner_server.cpp | 10 ++++++-- .../src/planning/planner_tester.cpp | 5 +++- .../src/planning/planner_tester.hpp | 4 ++- .../planning/test_planner_is_path_valid.cpp | 25 ++++++++++++++----- 8 files changed, 46 insertions(+), 13 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 index 8ba89879264..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 @@ -64,7 +64,10 @@ class IsPathValidCondition : public BT::ConditionNode return { BT::InputPort("path", "Path to Check"), BT::InputPort("server_timeout"), - BT::InputPort("max_cost", 255, "Maximum cost of the path") + BT::InputPort("max_cost", 253, "Maximum cost of the path"), + BT::InputPort( + "consider_unknown_as_obstacle", false, + "Whether to consider unknown cost as obstacle") }; } @@ -75,6 +78,7 @@ class IsPathValidCondition : public BT::ConditionNode // 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 785524b0085..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. @@ -284,6 +285,7 @@ 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 a72648cb2e0..8caed0c3971 100644 --- a/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp @@ -24,7 +24,7 @@ IsPathValidCondition::IsPathValidCondition( const std::string & condition_name, const BT::NodeConfiguration & conf) : BT::ConditionNode(condition_name, conf), - max_cost_(255) + max_cost_(253), consider_unknown_as_obstacle_(false) { node_ = config().blackboard->get("node"); client_ = node_->create_client("is_path_valid"); @@ -36,6 +36,7 @@ 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() @@ -51,6 +52,7 @@ BT::NodeStatus IsPathValidCondition::tick() 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 ae9e5eaf0fd..e178847085a 100644 --- a/nav2_msgs/srv/IsPathValid.srv +++ b/nav2_msgs/srv/IsPathValid.srv @@ -1,7 +1,8 @@ #Determine if the current path is still valid nav_msgs/Path path -uint8 max_cost 254 +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 6a827cf07a3..d1aa2343d01 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 > request->max_cost || 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 || cost > request->max_cost) { + } 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 4cfb0f32af4..eda7ce0b853 100644 --- a/nav2_system_tests/src/planning/planner_tester.cpp +++ b/nav2_system_tests/src/planning/planner_tester.cpp @@ -397,13 +397,16 @@ TaskStatus PlannerTester::createPlan( return TaskStatus::FAILED; } -bool PlannerTester::isPathValid(nav_msgs::msg::Path & path, unsigned int max_cost) +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 16264909ee2..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, unsigned int max_cost); + 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 68c38ca28bf..67c78b72b39 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,10 +32,11 @@ TEST(testIsPathValid, testIsPathValid) planner_tester->loadSimpleCostmap(TestCostmap::top_left_obstacle); nav_msgs::msg::Path path; - unsigned int max_cost = 255; + unsigned int max_cost = 253; + bool consider_unknown_as_obstacle = false; // empty path - bool is_path_valid = planner_tester->isPathValid(path, max_cost); + bool is_path_valid = planner_tester->isPathValid(path, max_cost, consider_unknown_as_obstacle); EXPECT_FALSE(is_path_valid); // invalid path @@ -47,10 +48,10 @@ TEST(testIsPathValid, testIsPathValid) path.poses.push_back(pose); } } - is_path_valid = planner_tester->isPathValid(path, max_cost); + is_path_valid = planner_tester->isPathValid(path, max_cost, consider_unknown_as_obstacle); EXPECT_FALSE(is_path_valid); - // valid path + // valid path, contains NO_INFORMATION(255) path.poses.clear(); for (float i = 0; i < 10; i += 1.0) { geometry_msgs::msg::PoseStamped pose; @@ -58,12 +59,24 @@ TEST(testIsPathValid, testIsPathValid) pose.pose.position.y = i; path.poses.push_back(pose); } - is_path_valid = planner_tester->isPathValid(path, max_cost); + is_path_valid = planner_tester->isPathValid(path, max_cost, consider_unknown_as_obstacle); EXPECT_TRUE(is_path_valid); + // valid path, 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); + is_path_valid = planner_tester->isPathValid(path, max_cost, consider_unknown_as_obstacle); EXPECT_FALSE(is_path_valid); } From 2c510d9ddff4b48a299abb977d448b4c73e24286 Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Wed, 29 Jan 2025 21:19:54 +0000 Subject: [PATCH 4/5] Edit comment Signed-off-by: mini-1235 --- nav2_system_tests/src/planning/test_planner_is_path_valid.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 67c78b72b39..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 @@ -51,7 +51,7 @@ TEST(testIsPathValid, testIsPathValid) is_path_valid = planner_tester->isPathValid(path, max_cost, consider_unknown_as_obstacle); EXPECT_FALSE(is_path_valid); - // valid path, contains NO_INFORMATION(255) + // valid path path.poses.clear(); for (float i = 0; i < 10; i += 1.0) { geometry_msgs::msg::PoseStamped pose; @@ -62,7 +62,7 @@ TEST(testIsPathValid, testIsPathValid) is_path_valid = planner_tester->isPathValid(path, max_cost, consider_unknown_as_obstacle); EXPECT_TRUE(is_path_valid); - // valid path, contains NO_INFORMATION(255) + // valid path, but contains NO_INFORMATION(255) path.poses.clear(); consider_unknown_as_obstacle = true; for (float i = 0; i < 10; i += 1.0) { From 4dc0a19dea60973dbb0547c960f1f0773d9c679d Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 29 Jan 2025 14:22:43 -0800 Subject: [PATCH 5/5] Update nav2_planner/src/planner_server.cpp Signed-off-by: Steve Macenski --- nav2_planner/src/planner_server.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index d1aa2343d01..03cf80fdf1c 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -701,7 +701,7 @@ void PlannerServer::isPathValid( 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) { + } else if (cost == nav2_costmap_2d::NO_INFORMATION) { cost = nav2_costmap_2d::FREE_SPACE; }