Skip to content

Commit

Permalink
mppi: return NO_INFORMATION when the checked point is outside the cos…
Browse files Browse the repository at this point in the history
…tmap (#3816)

otherwise the controller crashes at ObstaclesCritic::costAtPose
because x_i and y_i isn't initialized.
  • Loading branch information
981213 authored Sep 14, 2023
1 parent 61d70bc commit 6b250a7
Showing 1 changed file with 4 additions and 1 deletion.
5 changes: 4 additions & 1 deletion nav2_mppi_controller/src/critics/obstacles_critic.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -191,7 +191,10 @@ CollisionCost ObstaclesCritic::costAtPose(float x, float y, float theta)
float & cost = collision_cost.cost;
collision_cost.using_footprint = false;
unsigned int x_i, y_i;
collision_checker_.worldToMap(x, y, x_i, y_i);
if (!collision_checker_.worldToMap(x, y, x_i, y_i)) {
cost = nav2_costmap_2d::NO_INFORMATION;
return collision_cost;
}
cost = collision_checker_.pointCost(x_i, y_i);

if (consider_footprint_ && (cost >= possibly_inscribed_cost_ || possibly_inscribed_cost_ < 1)) {
Expand Down

0 comments on commit 6b250a7

Please sign in to comment.