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

Jump to bottom

Fix for robot footprint collision in obstacles critic (backport #3878) #3946

Merged
merged 1 commit into from
Nov 6, 2023
Merged

Fix for robot footprint collision in obstacles critic (backport #3878) #3946

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
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
18 changes: 16 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,11 @@ void ObstaclesCritic::score(CriticData & data)
return;
}

if (consider_footprint_) {
// footprint may have changed since initialization if user has dynamic footprints
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