Question about Costmap2D's getSizeInMeters functions

[DylanDeCoeyer-Quimesis]

Hi,

While debugging an issue, I ended up looking at the getSizeInMetersX() and getSizeInMetersY() of Costmap2D [1].

double Costmap2D::getSizeInMetersX() const
{
  return (size_x_ - 1 + 0.5) * resolution_;
}

double Costmap2D::getSizeInMetersY() const
{
  return (size_y_ - 1 + 0.5) * resolution_;
}

I don't get the purpose of the - 1 + 0.5. In my understanding, the size of the costmap in meters should be the number of cells multiplied by the resolution, nothing more. I tried to find the original commit adding those lines to look for an explanation, but those lines have been there since the origin of navigation 1.

Can someone help me understand this? If it's correct, an extra comment in the code would be appreciated.

[1] https://github.com/ros-navigation/navigation2/blob/main/nav2_costmap_2d/src/costmap_2d.cpp#L558

[DylanDeCoeyer-Quimesis]

I noticed that another equivalent issue exists in ros-planning/navigation, still unresolved : ros-planning/navigation#376

[SteveMacenski]

Do you see that it produces incorrect results?

I don't have an immediate answer for it. I was going to look into it but if both David and Fergs don't know, fat chance I'll come up with anything better than unsatisfying speculation.

and I cannot find any part of the main codebase that would be (negatively) affected.

I agree with this comment in the thread. I can't find a reason to fix it. Sometimes there's just legacy code that as long as nothing is broken, don't fix it. Eventually we'll have a deeper rewrite of costmap2D and in such a time these technical debts are removed.

[DylanDeCoeyer-Quimesis]

@SteveMacenski This comment is indeed interesting and responds to your first question:

This does appear to be a bug. Perhaps the old costmap code used this "feature" -- but the new code definitely does not (full search is here https://github.com/ros-planning/navigation/search?utf8=%E2%9C%93&q=getSizeInMetersX).

When using a rolling costmap, it looks like our costmap is off by 1/2 of a cell resolution all the time.

Most interestingly clearCostmap is also probably incorrect because of this (

navigation/clear_costmap_recovery/src/clear_costmap_recovery.cpp

Line 154 in 0d61d2a

costmap->addExtraBounds(ox, oy, ox + width, oy + height);
)
We certainly need to test a fix, but I think the proposed fix above is correct, and I cannot find any part of the main codebase that would be (negatively) affected.

I think that you wrongly interpreted the last sentence: it means that the proposed fix is not negatively affecting any part of the code. So the proposed fix should be integrated.

[SteveMacenski]

Do you see anywhere that's being negatively impacted today? I suppose building new costmap layers is a common application for professional users so I'm extremely cautious about making superficial changes to very legacy pieces of software like this that could create subtle regressions unless you can point out a specific use-case that this needs to be fixed to create and/or there's a bug somewhere in your system that this is the root cause of.

I'm definitely not saying that you're wrong, but there are certain things that I don't think are worth rocking the boat on unless we can point out an issue that it practically causes when maintaining legacy software.

I will say its seems consistent with mapToWorld

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_;
}

If we had a costmap that was 10x10 with resolution of 1m and origin of (0,0) just for illustration. getSizeInMetersX would return 9.5 and the mx=9 as the boundary for the last X would also return 9.5. That alignment makes me unincentivized to change things since that would create a gap that didn't previously exist.

[DylanDeCoeyer-Quimesis]

I understand your point. The issue which made me look into this is a (non systematic) persistent shift of my local costmap with respect to my robot (and with respect to my global costmap which is synced). Here is a typical example were the shift was ~40% of a cell along the Y axis in the negative direction. I already observed smaller shifts.

Here is a normal case, where the local costmap is perfectly centered on the robot (and coherent with the global costmap):

My rolling local costmap uses map as global frame. Since it's position is computed as:

double new_origin_x = robot_x - combined_costmap_.getSizeInMetersX() / 2;
double new_origin_y = robot_y - combined_costmap_.getSizeInMetersY() / 2;

If getSizeInMeters provides a costmap size which is shortened by 50% of a cell, it might explain why the local costmap is not positioned correctly. Still, the shift should be constant and systematic, while it's not in my case. So I'm still not sure of the root cause.

[SteveMacenski]

I'm not exactly sure what I'm looking at - but it looks to me like potentially a global and local costmap? I'm not sure what those costs are that are being represented. The rolling costmap is not going to be aligned with the global costmap if that costmap itself is also not rolling. There's going to be aliasing because the robot is always at the center of the local costmap (certainly we can discuss if that's really true) so the alignment in the frame / cells with a static map will never be true. The robot has a different initial odometric orientation and the robot's pose won't be in the center of a given cell. So, I don't find that visualization to be necessarily indicative of an issue.

What do you see if you change the implementation to be what you think it should be? I would not expect that you could obtain both of those images if there was a true static constant offset of 0.5 cells in both X and Y, as you mention. So I'm less convinced that this is the reason for that issue - but worth a quick test

[padhupradheep]

@DylanDeCoeyer-Quimesis any updates?

[SteveMacenski]

@DylanDeCoeyer-Quimesis any update?

[DylanDeCoeyer-Quimesis]

I'm not exactly sure what I'm looking at - but it looks to me like potentially a global and local costmap?

Yes, that's it.

I'm not sure what those costs are that are being represented.

It's the "costmap" color scheme on rviz2. So purple is a lethal obstacle, cyan is an inscribed obstacle, shades of red are non lethal areas to be avoided if possible.

The rolling costmap is not going to be aligned with the global costmap if that costmap itself is also not rolling. There's going to be aliasing because the robot is always at the center of the local costmap (certainly we can discuss if that's really true) so the alignment in the frame / cells with a static map will never be true.

In fact, the robot is not perfectly at the center of the local costmap due to the cast of center coordinates to integers.

void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw)
{
  // Lock for the remainder of this function, some plugins (e.g. VoxelLayer)
  // implement thread unsafe updateBounds() functions.
  std::unique_lock<Costmap2D::mutex_t> lock(*(combined_costmap_.getMutex()));

  // if we're using a rolling buffer costmap...
  // we need to update the origin using the robot's position
  if (rolling_window_) {
    double new_origin_x = robot_x - combined_costmap_.getSizeInMetersX() / 2;
    double new_origin_y = robot_y - combined_costmap_.getSizeInMetersY() / 2;
    primary_costmap_.updateOrigin(new_origin_x, new_origin_y);
    combined_costmap_.updateOrigin(new_origin_x, new_origin_y);
  }

void Costmap2D::updateOrigin(double new_origin_x, double new_origin_y)
{
  // project the new origin into the grid
  int cell_ox, cell_oy;
  cell_ox = static_cast<int>((new_origin_x - origin_x_) / resolution_);
  cell_oy = static_cast<int>((new_origin_y - origin_y_) / resolution_);

However, if both the local and global costmaps use the same global_frame ("map" in my case), their cells should be aligned. This is what I usually observe: the costmap jumps from one cell to the next, so the cells of global and local costmap are perfectly aligned.

Screencast.20from.2006-03-2025.2009.49.13.20AM.mp4

But in some cases, a slight shift appears and I still don't know why.

Fixing the getSizeInMeters functions as suggested in this thread did not help. The issue is likely dowstream.

[SteveMacenski]

If that doesn't resolve the error, should we rename this issue about costmap alignment ?

[DylanDeCoeyer-Quimesis]

No, I believe that the original question remains relevant. But I can certainly create a new issue for this misalignment.

[SteveMacenski]

I'm going to close this since that is the core of your concern and this topic is a specialization on it which could seed a direction of debugging based on these functions to see if it is a cause.