[Accessing] use costmap resolution.

sunava · Dec 10, 2024 · 6fbff05 · 6fbff05
1 parent 8b471bb
commit 6fbff05
Showing 1 changed file with 2 additions and 2 deletions.
diff --git a/src/pycram/designators/location_designator.py b/src/pycram/designators/location_designator.py
@@ -300,8 +300,8 @@ def adjust_map_for_drawer_opening(cost_map: Costmap, init_pose: Pose, goal_pose:
         orthogonal_vector /= np.linalg.norm(orthogonal_vector)
         orthogonal_size = width
         map_origin_idx = cost_map.map.shape[0] // 2, cost_map.map.shape[1] // 2
-        for i in range(int(motion_vector_length / 0.02)):
-            for j in range(int(orthogonal_size / 0.02)):
+        for i in range(int(motion_vector_length / cost_map.resolution)):
+            for j in range(int(orthogonal_size / cost_map.resolution)):
                 idx = (int(map_origin_idx[0] + i * unit_motion_vector[0] + j * orthogonal_vector[0]),
                        int(map_origin_idx[1] + i * unit_motion_vector[1] + j * orthogonal_vector[1]))
                 cost_map.map[idx] = 0