utilities.py 8.28 KB
 Aravind Bk committed Nov 17, 2018 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 ``````from .constants import * from . import road_geokinemetry as rd import numpy as np # Add classes for each kind of utility functions # Available utilities: # 1) BoundingBox: for working with bounding boxes # @brief: class BoundingBox(object): """A class that provides utility functions related to bounding boxes. * Bounding box: ndarray of shape (4,2) containing the vertices of the rectangle in order """ @staticmethod def does_bounding_box_intersect(first_bb, second_bb): """Checks if bounding boxes intersect using separating axis test: Two objects don't intersect if you can find a line that separates the two objects. Works only for quadrilateral bounding boxes (in 2D). Args: first_bb: np.ndarray First bounding box second_bb: np.ndarray Second bounding box Returns: true if there is an intersection """ for i, vertex in enumerate(first_bb): # for each vertex, get line joining it and next vertex line = np.array([vertex, first_bb[(i + 1) % 4]]) # check side of other 2 vertices in first_bb side_of_first_bb = BoundingBox.localize_point_wrt_line( `````` Ashish Gaurav committed Nov 19, 2018 38 39 40 `````` line, first_bb[(i + 2) % 4]) + BoundingBox.localize_point_wrt_line( line, first_bb[(i + 3) % 4]) `````` Aravind Bk committed Nov 17, 2018 41 42 43 44 45 `````` # Find side of all points in second_bb side_of_second_bb = [] for j in range(second_bb.shape[0]): side_of_second_bb.append( `````` Ashish Gaurav committed Nov 19, 2018 46 `````` BoundingBox.localize_point_wrt_line(line, second_bb[j, :])) `````` Aravind Bk committed Nov 17, 2018 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 `````` # Check if all points in second_bb are on same side of line # and that other vertices in first_bb are on the opposite side if BoundingBox.all_same_sign_in_list(side_of_second_bb) and ( side_of_first_bb * sum(side_of_second_bb) < 0): return False return True # TODO: This method was never used in any other places. Remove or keep it? @staticmethod def does_bounding_box_cross_line(line, bb): """Checks if bounding box crosses a line. Line is represented by an np.ndarray of size (2,2) representing two points in it. Works only for quadrilateral bounding boxes (in 2D). Args: line: np.ndarray Two points representing the line bb: np.ndarray Bounding box Returns: true if the bounding box crosses the line """ side_of_vertices = [] for i, vertex in enumerate(bb): side_of_vertices.append( BoundingBox.localize_point_wrt_line(line, vertex)) if BoundingBox.all_same_sign_in_list(side_of_vertices): return False else: return True @staticmethod def localize_bounding_box_wrt_line(line, bb): """Returns the side in which a bounding box is w.r.t a line. Line is represented by an np.ndarray of size (2,2) representing two points in it. Works only for quadrilateral bounding boxes (in 2D). Args: line: np.ndarray Two points representing the line bb: np.ndarray Bounding box Returns: 1 or -1 depending upon the side w.r.t the line. returns 0 if it intersects the line """ side_of_vertices = [] for i, vertex in enumerate(bb): side_of_vertices.append( BoundingBox.localize_point_wrt_line(line, vertex)) if BoundingBox.all_same_sign_in_list(side_of_vertices): for side in side_of_vertices: # side_of_vertices can contain 0 if a vertex touches the line if side != 0: return side else: return 0 @staticmethod def localize_point_wrt_line(line, testpoint): """Check the side in which a point lies w.r.t a line. Args: line: np.ndarray Two points representing the line testpoint: the point to be checked Returns: 1 or -1 depending upon the side w.r.t the edge; returns 0 if it is on the line """ edge = line[0, :] - line[1, :] vertex_in_edge = line[0, :] # find perpendicular to line rotated_edge = np.array([-edge[1], edge[0]]) # return -1 or 1 depening on the side. 0 if on the line return np.sign(rotated_edge[0] * (testpoint[0] - vertex_in_edge[0]) + rotated_edge[1] * (testpoint[1] - vertex_in_edge[1])) @staticmethod def all_same_sign_in_list(test_list): """Check if all elements in a list are of the same sign. Zero does not have a sign. Args: test_list: the python list to be checked Returns: 1 or -1 depending upon the side w.r.t the edge """ return all(k >= 0 for k in test_list) or all(k <= 0 for k in test_list) # Methods (get_APs, get_veh_attributes, config_Pyglet, # road2image, draw_all_shapes) related to Pyglet for graphical output. def get_APs(env, index, *args): """Retrieve atomic proposition data from the env. Args: env: SimpleIntersectionEnv object index: vehicle index args: atomic proposition names (variable arguments) Returns: multiline string with the required data """ properties = [] for arg in args: properties.append('%s: %s' % (arg, env.vehs[index].APs[arg])) return "\n" + "\n".join(properties) + "\n" # TODO: env and index info are not used for now. Remove or implement it. def get_veh_attributes(env, index, *args): """Retrieve vehicle attributes from env. Args: env: SimpleIntersectionEnv object index: vehicle index args: atomic proposition names (variable arguments) Returns: multiline string with the required data """ attributes = [] for arg in args: attributes.append( "%s: %s" % (arg, round(eval("env.vehs[index].%s" % arg), 2))) return "\n" + "\n".join(attributes) + "\n" def config_Pyglet(pyglet, win_x, win_y): """Configure pyglet and return the window that can be used by other pyglet objects. Args: win_x: x width of the window win_y: y width of the window Returns: Pyglet window object """ pyglet.options['debug_gl'] = False `````` Ashish Gaurav committed Nov 19, 2018 204 205 206 207 208 209 210 211 `````` # Use the no arguments version, if you don't have a GPU. Uncomment the # line below then. # config = pyglet.gl.Config() # Otherwise use the following line. config = pyglet.gl.Config(sample_buffers=1, samples=4) `````` Aravind Bk committed Nov 17, 2018 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 `````` window = pyglet.window.Window(width=win_x, height=win_y, config=config) return window def road2image(pos, which_dir): """Transform a 1D point to the 1D point in the graphics coordinate for the same direction ('h' or 'v'). Args: pos: 1D position in x or y-axis of the road coordinate system which_dir: the direction of the coordinate. It has to be either horizontal ('h') or vertical ('v'). Returns: the corresponding 1D position in the graphics coordinate. """ if which_dir is 'h': pos_out = (pos - rd.hlanes.start_pos) * H_SPACE_SCALE elif which_dir is 'v': pos_out = (pos - rd.vlanes.start_pos) * V_SPACE_SCALE pos_out = NUM_VPIXELS - pos_out else: raise AssertionError return pos_out def draw_all_shapes(shapes): """Go through all the shapes and call their draw function. Args: shapes: list of shapes """ for shape in shapes: shape.draw() `````` Ashish Gaurav committed Nov 19, 2018 249 `````` `````` Aravind Bk committed Nov 17, 2018 250 251 252 253 254 ``````# Methods (calculate_v_max, calculate_s) # used in generate_scenario of SimpleIntersectionEnv def calculate_v_max(dist): `````` Ashish Gaurav committed Nov 19, 2018 255 256 `````` """Calculate the maximum velocity you can reach at the given position ahead. `````` Aravind Bk committed Nov 17, 2018 257 258 259 260 261 262 263 264 265 266 267 `````` Args: dist: the distance you travel from the current position. Returns: the maximum reachable velocity. """ return np.sqrt(2.0 * MAX_ACCELERATION * max(dist, 0)) def calculate_s(v): `````` Ashish Gaurav committed Nov 19, 2018 268 269 `````` """Calculate the distance traveling when the vehicle is maximally de- accelerating for a complete stop. `````` Aravind Bk committed Nov 17, 2018 270 271 272 273 274 275 276 277 278 `````` Args: v: the current speed of the vehicle. Returns: the distance traveling when the vehicle is under the maximum de-acceleration to stop completely. """ return pow(v, 2) / MAX_ACCELERATION / 2.0``````