Local and global costmap ~half cell misalignment when both using the global frame

[DylanDeCoeyer-Quimesis]

Bug report

Required Info:

• Operating System:
  • Ubuntu 22.04
• Computer:
• ROS2 Version:
  • ROS2 Iron
• Version or commit hash:
  • 1.2.10
• DDS implementation:
  • Cyclone DDS

Steps to reproduce issue

Use a rolling local costmap and a static global costmap. Configure both with the same global frame. Typically:

local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 5.0
      publish_frequency: 5.0
      global_frame: map
      robot_base_frame: base_link
      rolling_window: true
      trinary_costmap: False
      width: 3
      height: 3
      resolution: 0.1
      robot_radius: 0.8
      use_maximum: true

      plugins: ["sync_layer"]
      sync_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
        map_topic: "/global_costmap/costmap"
        subscribe_to_updates: True
        enabled: True
      always_send_full_costmap: True

global_costmap:
  global_costmap:
    ros__parameters:
      publish_frequency: 1.0
      update_frequency: 5.0
      global_frame: map
      robot_base_frame: base_link
      robot_radius: 0.8
      resolution: 0.1
      track_unknown_space: False
      use_maximum: true

      plugins: [...]

Expected behavior

The local and global costmap are perfectly aligned since their sizes are integers, and the rolling costmap center coordinates is also casted to integer (https://github.com/ros-navigation/navigation2/blob/iron/nav2_costmap_2d/src/costmap_2d.cpp#L331).

450640192-408c6cfd-27ae-45d4-ae35-8edcbbaa872e.mp4

Actual behavior

In some cases / after some time (still unclear), the rolling local costmap appears slightly shifted (~40% of a cell) with respect to the global costmap.

Reproduction instructions

Additional information

Issue originally mentioned in #5152

[SteveMacenski]

Renamed for specificity - I hope that still meets your intent. Is this something you're going to look into or a report for some developer / maintainer to look into eventually when time or interest allots?

I also see you mention Iron, have you tested this on something more modern? I don't recall anything that would fix / brought up an issue like this recently, but it is worth checking that we didn't already fix this issue in main or main_humble (main branch compatible with Humble) so we don't chase our tails

[DylanDeCoeyer-Quimesis]

This shift is causing issues when our robot drives close to the border of the working area. Due to the shift, the position of the robot can be non-lethal on the global costmap and lethal on the local costmap. In this case, the planner can find a path, but our DWB follower systematically fails with nav2_core::NoValidControl.

As a consequence, I'll have to find a solution (either the root cause or a workaround). But I was hoping to get some insights from a Nav2 expert :-)

[SteveMacenski]

Understood, unfortunately I don't have a 'here's the reason' off of the top of my head. I would have to experiment a bit to gain some intuition about what's happening and what impacts the behavior. Personally what I'd start with is checking that its not an issue with visualization just as a starting point. Then:

• I'm suspect in this case of the origin calculation. The motion of the robot is not in discrete cells (continuous), so since we quash the origin change into an int, that would tell me there will be some approximation error. I'd check that first thing.
• One thing in that area that I would test is changing the calculation of new_grid_ox to use the continuous coordinates instead of the static casted int version. The static casted version of cell_ox is needed later in the function, but I'm not sure that the new grid origin information needs to have that approximation applied.
• I'm interested if we can see it only in the X or Y dimension and make some adjustments with respect to that axis only to see what helps / hurts and if that's related
• Certainly the X and Y bounds functions you mention in the other ticket are suspect, but if you're not saying that helped with anything, I wouldn't start there.

My off the cuff intuition would be to crutinize the origin since that is what the rest is offset based off of. The grid is a fixed size so unless you count the cells and see when this happen there's a row / column added or missing, then I don't think it has to do with calculating grid bounds, but rather where the grid itself is placed. I think that's the origin or incoming map data alignment related, possibly.