Skip to content

Fix for robot footprint collision in obstacles critic #3878

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 7 commits into from
Nov 6, 2023
Merged

Fix for robot footprint collision in obstacles critic #3878

Show file tree
Hide file tree
Changes from 5 commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
fa09c78
Inscribed/Circumscribed costs must be updated to take into account the
Oct 11, 2023
cef7a11
Merge branch 'main' into users/peanut-recalculate-radius
Nov 3, 2023
c201121
Add early return to avoid calculations if footprint has not changed.
Nov 3, 2023
9d6076e
Only update radius if using footprint.
Nov 4, 2023
69293f4
Remove perf timers.
Nov 5, 2023
42f5300
Merge branch 'main' into users/peanut-recalculate-radius
Nov 6, 2023
8ec14e0
Update comments.
Nov 6, 2023
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
1 change: 1 addition & 0 deletions nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -92,6 +92,7 @@ class ObstaclesCritic : public CriticFunction
float possibly_inscribed_cost_;
float collision_margin_distance_;
float near_goal_distance_;
float circumscribed_cost_{0}, circumscribed_radius_{0};

unsigned int power_{0};
float repulsion_weight_, critical_weight_{0};
Expand Down
19 changes: 17 additions & 2 deletions nav2_mppi_controller/src/critics/obstacles_critic.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -55,6 +55,13 @@ float ObstaclesCritic::findCircumscribedCost(
{
double result = -1.0;
bool inflation_layer_found = false;

const double circum_radius = costmap->getLayeredCostmap()->getCircumscribedRadius();
if (static_cast<float>(circum_radius) == circumscribed_radius_) {
// early return if footprint size is unchanged
return circumscribed_cost_;
}

// check if the costmap has an inflation layer
for (auto layer = costmap->getLayeredCostmap()->getPlugins()->begin();
layer != costmap->getLayeredCostmap()->getPlugins()->end();
Expand All @@ -66,7 +73,6 @@ float ObstaclesCritic::findCircumscribedCost(
}

inflation_layer_found = true;
const double circum_radius = costmap->getLayeredCostmap()->getCircumscribedRadius();
const double resolution = costmap->getCostmap()->getResolution();
result = inflation_layer->computeCost(circum_radius / resolution);
inflation_scale_factor_ = static_cast<float>(inflation_layer->getCostScalingFactor());
Expand All @@ -83,7 +89,10 @@ float ObstaclesCritic::findCircumscribedCost(
"significantly slow down planning times and not avoid anything but absolute collisions!");
}

return static_cast<float>(result);
circumscribed_radius_ = static_cast<float>(circum_radius);
circumscribed_cost_ = static_cast<float>(result);

return circumscribed_cost_;
}

float ObstaclesCritic::distanceToObstacle(const CollisionCost & cost)
Expand All @@ -108,6 +117,12 @@ void ObstaclesCritic::score(CriticData & data)
return;
}

if (consider_footprint_) {
// footprint has almost certainly changed since initialize()
// and may continue to change
possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_);
}

// If near the goal, don't apply the preferential term since the goal is near obstacles
bool near_goal = false;
if (utils::withinPositionGoalTolerance(near_goal_distance_, data.state.pose.pose, data.path)) {
Expand Down