From 6fbff056cba0c56358340d1a025c73cc6272b64e Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Tue, 10 Dec 2024 10:33:50 +0100 Subject: [PATCH] [Accessing] use costmap resolution. --- src/pycram/designators/location_designator.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/pycram/designators/location_designator.py b/src/pycram/designators/location_designator.py index 3962bc685..bbfc108c3 100644 --- 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