Fixed bug causing global planner to plan to wrong cell

[siferati]

Description

Fixed a bug that was causing the global planner to compute paths to the wrong cell.
This allowed the global planner to return paths that were in collision.
You can see in the picture below how the global plan clearly ends inside the inscribed region of the costmap (i.e. collision!)

When the param old_navfn_behavior was set to false, the worldToMap function was subtracting 0.5 from the resulting cell coordinates, which is wrong. Although it makes sense to add 0.5 when converting from map to world (in order to return the center of the cell rather than the corner), the opposite is not true!

You can also confirm how in Costmap2D::mapToWorld we add 0.5 to get the center of the cell, but we don't subtract 0.5 when doing the opposite operation in Costmap2D::worldToMap.

navigation/costmap_2d/src/costmap_2d.cpp

Lines 202 to 206 in 47c9c62

	void Costmap2D::mapToWorld(unsigned int mx, unsigned int my, double& wx, double& wy) const
	{
	wx = origin_x_ + (mx + 0.5) * resolution_;
	wy = origin_y_ + (my + 0.5) * resolution_;
	}

navigation/costmap_2d/src/costmap_2d.cpp

Lines 208 to 220 in 47c9c62

	bool Costmap2D::worldToMap(double wx, double wy, unsigned int& mx, unsigned int& my) const
	{
	if (wx < origin_x_ || wy < origin_y_)
	return false;
	mx = (int)((wx - origin_x_) / resolution_);
	my = (int)((wy - origin_y_) / resolution_);
	if (mx < size_x_ && my < size_y_)
	return true;
	return false;
	}