Skip to content

Commit d101475

Browse files
committed
Merge branch 'feature/masked-replace-enhancements' into ros2
2 parents c1637f7 + 5330e70 commit d101475

File tree

1 file changed

+17
-1
lines changed

1 file changed

+17
-1
lines changed

elevation_mapping_cupy/elevation_mapping_cupy/elevation_mapping.py

Lines changed: 17 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -859,7 +859,23 @@ def get_map_with_name_ref(self, name, data):
859859
else:
860860
print("Layer {} is not in the map".format(name))
861861
return
862-
# Need 180 degree rotation to match coordinate system
862+
# Transform to align elevation_mapping_cupy with grid_map coordinate convention.
863+
#
864+
# elevation_mapping_cupy uses Row=Y, Col=X (see kernels/custom_kernels.py:35)
865+
# grid_map uses Row→-X, Col→-Y (see grid_map_core/src/GridMapMath.cpp:64-67
866+
# transformBufferOrderToMapFrame returns {-index[0], -index[1]})
867+
#
868+
# Required transformation:
869+
# 1. Transpose: swap axes so Row=X, Col=Y (matching grid_map's axis assignment)
870+
# 2. Flip axis 0: so increasing row → decreasing X (matching grid_map's -X)
871+
# 3. Flip axis 1: so increasing col → decreasing Y (matching grid_map's -Y)
872+
#
873+
# This is equivalent to: rot90(m.T, k=2) or flip(flip(m.T, 0), 1)
874+
#
875+
# Old 180° rotation (incorrect - missing transpose, caused 90° CCW error in RViz):
876+
# m = xp.flip(m, 0)
877+
# m = xp.flip(m, 1)
878+
m = m.T
863879
m = xp.flip(m, 0)
864880
m = xp.flip(m, 1)
865881
if use_stream:

0 commit comments

Comments
 (0)