Skip to content

Commit 6b250a7

Browse files
authored
mppi: return NO_INFORMATION when the checked point is outside the costmap (ros-navigation#3816)
otherwise the controller crashes at ObstaclesCritic::costAtPose because x_i and y_i isn't initialized.
1 parent 61d70bc commit 6b250a7

File tree

1 file changed

+4
-1
lines changed

1 file changed

+4
-1
lines changed

nav2_mppi_controller/src/critics/obstacles_critic.cpp

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -191,7 +191,10 @@ CollisionCost ObstaclesCritic::costAtPose(float x, float y, float theta)
191191
float & cost = collision_cost.cost;
192192
collision_cost.using_footprint = false;
193193
unsigned int x_i, y_i;
194-
collision_checker_.worldToMap(x, y, x_i, y_i);
194+
if (!collision_checker_.worldToMap(x, y, x_i, y_i)) {
195+
cost = nav2_costmap_2d::NO_INFORMATION;
196+
return collision_cost;
197+
}
195198
cost = collision_checker_.pointCost(x_i, y_i);
196199

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

0 commit comments

Comments
 (0)