Skip to content

traversable_map

TraversableMap

Bases: BaseMap

Traversable scene class. Contains the functionalities for navigation such as shortest path computation

Source code in omnigibson/maps/traversable_map.py 
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
class TraversableMap(BaseMap):
    """
    Traversable scene class.
    Contains the functionalities for navigation such as shortest path computation
    """

    def __init__(
        self,
        map_resolution=0.1,
        default_erosion_radius=0.0,
        trav_map_with_objects=True,
        num_waypoints=10,
        waypoint_resolution=0.2,
    ):
        """
        Args:
            map_resolution (float): map resolution in meters, each pixel represents this many meters;
                                    normally, this should be between 0.01 and 0.1
            default_erosion_radius (float): default map erosion radius in meters
            trav_map_with_objects (bool): whether to use objects or not when constructing graph
            num_waypoints (int): number of way points returned
            waypoint_resolution (float): resolution of adjacent way points
        """
        # Set internal values
        self.map_default_resolution = 0.01  # each pixel == 0.01m in the dataset representation
        self.default_erosion_radius = default_erosion_radius
        self.trav_map_with_objects = trav_map_with_objects
        self.num_waypoints = num_waypoints
        self.waypoint_interval = int(waypoint_resolution / map_resolution)

        # Values loaded at runtime
        self.trav_map_original_size = None
        self.trav_map_size = None
        self.mesh_body_id = None
        self.floor_heights = None
        self.floor_map = None

        # Run super method
        super().__init__(map_resolution=map_resolution)

    def _load_map(self, maps_path, floor_heights=(0.0,)):
        """
        Loads the traversability maps for all floors

        Args:
            maps_path (str): Path to the folder containing the traversability maps
            floor_heights (n-array): Height(s) of the floors for this map

        Returns:
            int: Size of the loaded map
        """
        if not os.path.exists(maps_path):
            log.warning("trav map does not exist: {}".format(maps_path))
            return

        self.floor_heights = floor_heights
        self.floor_map = []
        map_size = None
        for floor in range(len(self.floor_heights)):
            if self.trav_map_with_objects:
                # TODO: Shouldn't this be generated dynamically?
                trav_map = np.array(Image.open(os.path.join(maps_path, "floor_trav_{}.png".format(floor))))
            else:
                trav_map = np.array(Image.open(os.path.join(maps_path, "floor_trav_no_obj_{}.png".format(floor))))

            # If we do not initialize the original size of the traversability map, we obtain it from the image
            # Then, we compute the final map size as the factor of scaling (default_resolution/resolution) times the
            # original map size
            if self.trav_map_original_size is None:
                height, width = trav_map.shape
                assert height == width, "trav map is not a square"
                self.trav_map_original_size = height
                map_size = int(
                    self.trav_map_original_size * self.map_default_resolution / self.map_resolution
                )

            # We resize the traversability map to the new size computed before
            trav_map = cv2.resize(trav_map, (map_size, map_size))

            # We make the pixels of the image to be either 0 or 255
            trav_map[trav_map < 255] = 0

            self.floor_map.append(trav_map)

        return map_size

    @property
    def n_floors(self):
        """
        Returns:
            int: Number of floors belonging to this map's associated scene
        """
        return len(self.floor_heights)

    def _erode_trav_map(self, trav_map, robot=None):
        # Erode the traversability map to account for the robot's size
        if robot:
            robot_chassis_extent = robot.reset_joint_pos_aabb_extent[:2]
            radius = np.linalg.norm(robot_chassis_extent) / 2.0
        else:
            radius = self.default_erosion_radius
        radius_pixel = int(np.ceil(radius / self.map_resolution))
        trav_map = cv2.erode(trav_map, np.ones((radius_pixel, radius_pixel)))
        return trav_map

    def get_random_point(self, floor=None, reference_point=None, robot=None):
        """
        Sample a random point on the given floor number. If not given, sample a random floor number.
        If @reference_point is given, sample a point in the same connected component as the previous point.

        Args:
            floor (None or int): floor number. None means the floor is randomly sampled
                                 Warning: if @reference_point is given, @floor must be given; 
                                          otherwise, this would lead to undefined behavior
            reference_point (3-array): (x,y,z) if given, sample a point in the same connected component as this point

        Returns:
            2-tuple:
                - int: floor number. This is the sampled floor number if @floor is None
                - 3-array: (x,y,z) randomly sampled point
        """
        if reference_point is not None:
            assert floor is not None, "floor must be given if reference_point is given"

        # If nothing is given, sample a random floor and a random point on that floor
        if floor is None and reference_point is None:
            floor = np.random.randint(0, self.n_floors)

        # create a deep copy so that we don't erode the original map
        trav_map = self.floor_map[floor].copy()
        trav_map = self._erode_trav_map(trav_map, robot=robot)

        if reference_point is not None:
            # Find connected component
            _, component_labels = cv2.connectedComponents(trav_map, connectivity=4)

            # If previous point is given, sample a point in the same connected component
            prev_xy_map = self.world_to_map(reference_point[:2])
            prev_label = component_labels[prev_xy_map[0]][prev_xy_map[1]]
            trav_space = np.where(component_labels == prev_label)
        else:
            trav_space = np.where(trav_map == 255)
        idx = np.random.randint(0, high=trav_space[0].shape[0])
        xy_map = np.array([trav_space[0][idx], trav_space[1][idx]])
        x, y = self.map_to_world(xy_map)
        z = self.floor_heights[floor]
        return floor, np.array([x, y, z])

    def get_shortest_path(self, floor, source_world, target_world, entire_path=False, robot=None):
        """
        Get the shortest path from one point to another point.
        If any of the given point is not in the graph, add it to the graph and
        create an edge between it to its closest node.

        Args:
            floor (int): floor number
            source_world (2-array): (x,y) 2D source location in world reference frame (metric)
            target_world (2-array): (x,y) 2D target location in world reference frame (metric)
            entire_path (bool): whether to return the entire path
            robot (None or BaseRobot): if given, erode the traversability map to account for the robot's size

        Returns:
            2-tuple:
                - (N, 2) array: array of path waypoints, where N is the number of generated waypoints
                - float: geodesic distance of the path
        """
        source_map = tuple(self.world_to_map(source_world))
        target_map = tuple(self.world_to_map(target_world))

        # create a deep copy so that we don't erode the original map
        trav_map = self.floor_map[floor].copy()

        trav_map = self._erode_trav_map(trav_map, robot=robot)

        path_map = astar(trav_map, source_map, target_map)
        if path_map is None:
            # No traversable path found
            return None, None
        path_world = self.map_to_world(path_map)
        geodesic_distance = np.sum(np.linalg.norm(path_world[1:] - path_world[:-1], axis=1))
        path_world = path_world[:: self.waypoint_interval]

        if not entire_path:
            path_world = path_world[: self.num_waypoints]
            num_remaining_waypoints = self.num_waypoints - path_world.shape[0]
            if num_remaining_waypoints > 0:
                remaining_waypoints = np.tile(target_world, (num_remaining_waypoints, 1))
                path_world = np.concatenate((path_world, remaining_waypoints), axis=0)

        return path_world, geodesic_distance

n_floors property

Returns:

Name Type Description
int

Number of floors belonging to this map's associated scene

__init__(map_resolution=0.1, default_erosion_radius=0.0, trav_map_with_objects=True, num_waypoints=10, waypoint_resolution=0.2)

Parameters:

Name Type Description Default
map_resolution float

map resolution in meters, each pixel represents this many meters; normally, this should be between 0.01 and 0.1

 0.1
default_erosion_radius float

default map erosion radius in meters

 0.0
trav_map_with_objects bool

whether to use objects or not when constructing graph

 True
num_waypoints int

number of way points returned

 10
waypoint_resolution float

resolution of adjacent way points

 0.2
Source code in omnigibson/maps/traversable_map.py 
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
def __init__(
    self,
    map_resolution=0.1,
    default_erosion_radius=0.0,
    trav_map_with_objects=True,
    num_waypoints=10,
    waypoint_resolution=0.2,
):
    """
    Args:
        map_resolution (float): map resolution in meters, each pixel represents this many meters;
                                normally, this should be between 0.01 and 0.1
        default_erosion_radius (float): default map erosion radius in meters
        trav_map_with_objects (bool): whether to use objects or not when constructing graph
        num_waypoints (int): number of way points returned
        waypoint_resolution (float): resolution of adjacent way points
    """
    # Set internal values
    self.map_default_resolution = 0.01  # each pixel == 0.01m in the dataset representation
    self.default_erosion_radius = default_erosion_radius
    self.trav_map_with_objects = trav_map_with_objects
    self.num_waypoints = num_waypoints
    self.waypoint_interval = int(waypoint_resolution / map_resolution)

    # Values loaded at runtime
    self.trav_map_original_size = None
    self.trav_map_size = None
    self.mesh_body_id = None
    self.floor_heights = None
    self.floor_map = None

    # Run super method
    super().__init__(map_resolution=map_resolution)

get_random_point(floor=None, reference_point=None, robot=None)

Sample a random point on the given floor number. If not given, sample a random floor number. If @reference_point is given, sample a point in the same connected component as the previous point.

Parameters:

Name Type Description Default
floor None or int

floor number. None means the floor is randomly sampled Warning: if @reference_point is given, @floor must be given; otherwise, this would lead to undefined behavior

 None
reference_point 3 - array

(x,y,z) if given, sample a point in the same connected component as this point

 None

Returns:

Type Description

2-tuple: - int: floor number. This is the sampled floor number if @floor is None - 3-array: (x,y,z) randomly sampled point
Source code in omnigibson/maps/traversable_map.py 
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
def get_random_point(self, floor=None, reference_point=None, robot=None):
    """
    Sample a random point on the given floor number. If not given, sample a random floor number.
    If @reference_point is given, sample a point in the same connected component as the previous point.

    Args:
        floor (None or int): floor number. None means the floor is randomly sampled
                             Warning: if @reference_point is given, @floor must be given; 
                                      otherwise, this would lead to undefined behavior
        reference_point (3-array): (x,y,z) if given, sample a point in the same connected component as this point

    Returns:
        2-tuple:
            - int: floor number. This is the sampled floor number if @floor is None
            - 3-array: (x,y,z) randomly sampled point
    """
    if reference_point is not None:
        assert floor is not None, "floor must be given if reference_point is given"

    # If nothing is given, sample a random floor and a random point on that floor
    if floor is None and reference_point is None:
        floor = np.random.randint(0, self.n_floors)

    # create a deep copy so that we don't erode the original map
    trav_map = self.floor_map[floor].copy()
    trav_map = self._erode_trav_map(trav_map, robot=robot)

    if reference_point is not None:
        # Find connected component
        _, component_labels = cv2.connectedComponents(trav_map, connectivity=4)

        # If previous point is given, sample a point in the same connected component
        prev_xy_map = self.world_to_map(reference_point[:2])
        prev_label = component_labels[prev_xy_map[0]][prev_xy_map[1]]
        trav_space = np.where(component_labels == prev_label)
    else:
        trav_space = np.where(trav_map == 255)
    idx = np.random.randint(0, high=trav_space[0].shape[0])
    xy_map = np.array([trav_space[0][idx], trav_space[1][idx]])
    x, y = self.map_to_world(xy_map)
    z = self.floor_heights[floor]
    return floor, np.array([x, y, z])

get_shortest_path(floor, source_world, target_world, entire_path=False, robot=None)

Get the shortest path from one point to another point. If any of the given point is not in the graph, add it to the graph and create an edge between it to its closest node.

Parameters:

Name Type Description Default
floor int

floor number

 required
source_world 2 - array

(x,y) 2D source location in world reference frame (metric)

 required
target_world 2 - array

(x,y) 2D target location in world reference frame (metric)

 required
entire_path bool

whether to return the entire path

 False
robot None or BaseRobot

if given, erode the traversability map to account for the robot's size

 None

Returns:

Type Description

2-tuple: - (N, 2) array: array of path waypoints, where N is the number of generated waypoints - float: geodesic distance of the path
Source code in omnigibson/maps/traversable_map.py 
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
def get_shortest_path(self, floor, source_world, target_world, entire_path=False, robot=None):
    """
    Get the shortest path from one point to another point.
    If any of the given point is not in the graph, add it to the graph and
    create an edge between it to its closest node.

    Args:
        floor (int): floor number
        source_world (2-array): (x,y) 2D source location in world reference frame (metric)
        target_world (2-array): (x,y) 2D target location in world reference frame (metric)
        entire_path (bool): whether to return the entire path
        robot (None or BaseRobot): if given, erode the traversability map to account for the robot's size

    Returns:
        2-tuple:
            - (N, 2) array: array of path waypoints, where N is the number of generated waypoints
            - float: geodesic distance of the path
    """
    source_map = tuple(self.world_to_map(source_world))
    target_map = tuple(self.world_to_map(target_world))

    # create a deep copy so that we don't erode the original map
    trav_map = self.floor_map[floor].copy()

    trav_map = self._erode_trav_map(trav_map, robot=robot)

    path_map = astar(trav_map, source_map, target_map)
    if path_map is None:
        # No traversable path found
        return None, None
    path_world = self.map_to_world(path_map)
    geodesic_distance = np.sum(np.linalg.norm(path_world[1:] - path_world[:-1], axis=1))
    path_world = path_world[:: self.waypoint_interval]

    if not entire_path:
        path_world = path_world[: self.num_waypoints]
        num_remaining_waypoints = self.num_waypoints - path_world.shape[0]
        if num_remaining_waypoints > 0:
            remaining_waypoints = np.tile(target_world, (num_remaining_waypoints, 1))
            path_world = np.concatenate((path_world, remaining_waypoints), axis=0)

    return path_world, geodesic_distance