navigation
navigation copied to clipboard
copyCostmapWindow function does not use size parameters correctly
If the size of the costmap passed to copyCostmapWindow
is (m,n) with origin (0,0) and when the size of the input costmap is (m,n). The current implementation fails and returns a false at
// compute the bounds of our new map
unsigned int lower_left_x, lower_left_y, upper_right_x, upper_right_y;
if (!map.worldToMap(win_origin_x, win_origin_y, lower_left_x, lower_left_y)
|| !map.worldToMap(win_origin_x + win_size_x, win_origin_y + win_size_y, upper_right_x, upper_right_y))
{
// ROS_ERROR("Cannot window a map that the window bounds don't fit inside of");
return false;
}
This should not be the case. This condition should pass and the size of the costmap set should be incremented by 1. Currently it is
size_x_ = upper_right_x - lower_left_x;
size_y_ = upper_right_y - lower_left_y;
It should be
size_x_ = upper_right_x - lower_left_x+1;
size_y_ = upper_right_y - lower_left_y+1;