Skip to content

Add ispathvalid maxcost #4873

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 5 commits into from
Jan 29, 2025
Merged

Add ispathvalid maxcost #4873

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
969c34c
Add ispathvalid maxcost
mini-1235 Jan 25, 2025
4a81119
Fix problems as requested
mini-1235 Jan 28, 2025
342378e
Set default as 253, Add considered unknown as obstacle
mini-1235 Jan 29, 2025
2c510d9
Edit comment
mini-1235 Jan 29, 2025
4dc0a19
Update nav2_planner/src/planner_server.cpp
SteveMacenski Jan 29, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 7 additions & 1 deletion nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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")
};
}

Expand All @@ -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
Expand Down
3 changes: 3 additions & 0 deletions nav2_behavior_tree/nav2_tree_nodes.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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>

Expand Down Expand Up @@ -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">
Expand Down
7 changes: 6 additions & 1 deletion nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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");
Expand All @@ -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()
Expand All @@ -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_) ==
Expand Down
2 changes: 2 additions & 0 deletions nav2_msgs/srv/IsPathValid.srv
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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
10 changes: 8 additions & 2 deletions nav2_planner/src/planner_server.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Copy link
Contributor

@tonynajjar tonynajjar Jun 11, 2025 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think this introduced a bug or at the very least an unexpected behavior compared to how it was.

In this clause, use_radius is false so cost is a footprintCost. The default max_cost that was set to 253. A footprintCost of 253 is acceptable (a vertex is intersecting a INSCRIBED_INFLATED_OBSTACLE cell) but according to this logic, this renders the path invalid.

@mini-1235 can you double-check my logic? Thank you

Copy link
Member

@SteveMacenski SteveMacenski Jun 11, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is true, but that's why this is a configuration of the request. If you set use_radius = false then you should set your max_cost limit.

Do you recommend changing the default to 254? That seems sensible enough to me so it doesn't trigger anything unless the user meaningfully overrides it with another value of their selection.

Copy link
Contributor

@tonynajjar tonynajjar Jun 11, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Do you recommend changing the default to 254?

Yes and that would not change anything for the case of where use_radius is true because we already check cost == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE)

This is true, but that's why this is a configuration of the request

I just didn't expect a change in behavior in my IsPathValid BT node, at least not without notes.

response->is_valid = false;
break;
}
Expand Down
6 changes: 5 additions & 1 deletion nav2_system_tests/src/planning/planner_tester.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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");
Expand Down
4 changes: 3 additions & 1 deletion nav2_system_tests/src/planning/planner_tester.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down
25 changes: 22 additions & 3 deletions nav2_system_tests/src/planning/test_planner_is_path_valid.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand All @@ -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)
Expand Down
Loading