Skip to content
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

Add ispathvalid maxcost #4873

Merged
merged 5 commits into from
Jan 29, 2025
+56 −9
Merged

Add ispathvalid maxcost #4873

Show file tree
Hide file tree
Changes from 2 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
4 changes: 3 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,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<unsigned int>("max_cost", 255, "Maximum cost of the path")
SteveMacenski marked this conversation as resolved.
Show resolved Hide resolved
};
}

Expand All @@ -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_;
unsigned int max_cost_;
};

} // namespace nav2_behavior_tree
Expand Down
1 change: 1 addition & 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 @@ -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>
SteveMacenski marked this conversation as resolved.
Show resolved Hide resolved
</Condition>

<Condition ID="WouldAControllerRecoveryHelp">
Expand Down
5 changes: 4 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_(255)
SteveMacenski marked this conversation as resolved.
Show resolved Hide resolved
{
node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
client_ = node_->create_client<nav2_msgs::srv::IsPathValid>("is_path_valid");
Expand All @@ -34,6 +35,7 @@ IsPathValidCondition::IsPathValidCondition(
void IsPathValidCondition::initialize()
{
getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);
getInput<unsigned int>("max_cost", max_cost_);
}

BT::NodeStatus IsPathValidCondition::tick()
Expand All @@ -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_) ==
Expand Down
1 change: 1 addition & 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,7 @@
#Determine if the current path is still valid

nav_msgs/Path path
uint8 max_cost 254
---
bool is_valid
int32[] invalid_pose_indices
4 changes: 2 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 @@ -700,12 +700,12 @@ void PlannerServer::isPathValid(
}

if (use_radius &&
(cost == nav2_costmap_2d::LETHAL_OBSTACLE ||
(cost > request->max_cost || cost == nav2_costmap_2d::LETHAL_OBSTACLE ||
SteveMacenski marked this conversation as resolved.
Show resolved Hide resolved
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) {
SteveMacenski marked this conversation as resolved.
Show resolved Hide resolved
Copy link
Member

Choose a reason for hiding this comment

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

Suggested change
} else if (cost == nav2_costmap_2d::LETHAL_OBSTACLE || cost > request->max_cost) {
} else if (cost == nav2_costmap_2d::LETHAL_OBSTACLE || cost >= request->max_cost) {

Copy link
Contributor Author

@mini-1235 mini-1235 Jan 29, 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 we treat NO_INFORMATION(255) as FREE_SPACE here, so I should convert 255 to 0 / do a simple && cost!=FREE_SAPCE here, am I correct?

Copy link
Member

@SteveMacenski SteveMacenski Jan 29, 2025 •
edited
Loading

Choose a reason for hiding this comment

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

This is a good point - I think if we add in max_cost, we should also add in a bool service argument for unknown_valid and we check if the cost is larger than the max cost and if unknown should be considered valid or invalid (collision). It would need to be exposed from the BT node and so forth as well - but easy enough!

response->is_valid = false;
break;
}
Expand Down
3 changes: 2 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,13 @@ 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)
{
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");
Expand Down
2 changes: 1 addition & 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,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, unsigned int max_cost);

private:
void setCostmap();
Expand Down
12 changes: 9 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,10 @@ TEST(testIsPathValid, testIsPathValid)
planner_tester->loadSimpleCostmap(TestCostmap::top_left_obstacle);

nav_msgs::msg::Path path;
unsigned int max_cost = 255;
SteveMacenski marked this conversation as resolved.
Show resolved Hide resolved

// 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
Expand All @@ -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
Expand All @@ -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)
Expand Down
Loading