Skip to content

Commit

Permalink
Fix problems as requested
Browse files Browse the repository at this point in the history
Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>
  • Loading branch information
mini-1235 committed Jan 28, 2025
1 parent 969c34c commit 4a81119
Show file tree
Hide file tree
Showing 7 changed files with 8 additions and 8 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -64,7 +64,7 @@ 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<uint8_t>("max_cost", 255, "Maximum cost of the path")
BT::InputPort<unsigned int>("max_cost", 255, "Maximum cost of the path")
};
}

Expand All @@ -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
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,7 @@ IsPathValidCondition::IsPathValidCondition(
void IsPathValidCondition::initialize()
{
getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);
getInput<uint8_t>("max_cost", max_cost_);
getInput<unsigned int>("max_cost", max_cost_);
}

BT::NodeStatus IsPathValidCondition::tick()
Expand Down
2 changes: 1 addition & 1 deletion nav2_msgs/srv/IsPathValid.srv
Original file line number Diff line number Diff line change
@@ -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
2 changes: 1 addition & 1 deletion nav2_planner/src/planner_server.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
2 changes: 1 addition & 1 deletion nav2_system_tests/src/planning/planner_tester.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
2 changes: 1 addition & 1 deletion nav2_system_tests/src/planning/planner_tester.hpp
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, uint8_t max_cost);
bool isPathValid(nav_msgs::msg::Path & path, unsigned int max_cost);

private:
void setCostmap();
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down

0 comments on commit 4a81119

Please sign in to comment.