def heading_diff(self, target_lane): lateral = None if isinstance(target_lane, StraightLane): lateral = np.asarray( get_vertical_vector(target_lane.end - target_lane.start)[1]) elif isinstance(target_lane, CircularLane): if target_lane.direction == -1: lateral = self.position - target_lane.center else: lateral = target_lane.center - self.position elif isinstance(target_lane, WayPointLane): lane_segment = target_lane.segment( target_lane.local_coordinates(self.position)[0]) lateral = lane_segment["lateral_direction"] lateral_norm = norm(lateral[0], lateral[1]) forward_direction = self.heading # print(f"Old forward direction: {self.forward_direction}, new heading {self.heading}") forward_direction_norm = norm(forward_direction[0], forward_direction[1]) if not lateral_norm * forward_direction_norm: return 0 cos = ((forward_direction[0] * lateral[0] + forward_direction[1] * lateral[1]) / (lateral_norm * forward_direction_norm)) # return cos # Normalize to 0, 1 return clip(cos, -1.0, 1.0) / 2 + 0.5
def heading_diff(self, target_lane): lateral = None if isinstance(target_lane, StraightLane): lateral = np.asarray( get_vertical_vector(target_lane.end - target_lane.start)[1]) elif isinstance(target_lane, CircularLane): if target_lane.direction == -1: lateral = self.position - target_lane.center else: lateral = target_lane.center - self.position else: raise ValueError("Unknown target lane type: {}".format( type(target_lane))) lateral_norm = norm(lateral[0], lateral[1]) forward_direction = self.heading # print(f"Old forward direction: {self.forward_direction}, new heading {self.heading}") forward_direction_norm = norm(forward_direction[0], forward_direction[1]) if not lateral_norm * forward_direction_norm: return 0 cos = ((forward_direction[0] * lateral[0] + forward_direction[1] * lateral[1]) / (lateral_norm * forward_direction_norm)) # return cos # Normalize to 0, 1 return clip(cos, -1.0, 1.0) / 2 + 0.5
def test_utils(): from pgdrive.utils.math_utils import safe_clip, safe_clip_for_small_array, get_vertical_vector, distance_greater arr = np.array([ np.nan, np.inf, -np.inf, 1000000000000000000000000000, -10000000000000000, 0, 1 ]) ans = np.array([0, 1, -1, 1, -1, 0, 1]) assert np.array_equal(safe_clip(arr, -1, 1), ans) assert np.array_equal(safe_clip_for_small_array(arr, -1, 1), ans) assert np.dot(get_vertical_vector([0, 1])[0], [0, 1]) == 0 assert np.dot(get_vertical_vector([0, 1])[1], [0, 1]) == 0 for _ in range(20): pos = np.random.normal(0, 1, size=(2, )) assert distance_greater(pos, (0, 0), 0.5) == (abs(np.linalg.norm(pos, ord=2)) > 0.5)
def projection(self, vector): # Projected to the heading of vehicle # forward_vector = self.vehicle.get_forward_vector() # forward_old = (forward_vector[0], -forward_vector[1]) forward = self.heading # print(f"[projection] Old forward {forward_old}, new heading {forward}") norm_velocity = norm(forward[0], forward[1]) + 1e-6 project_on_heading = (vector[0] * forward[0] + vector[1] * forward[1]) / norm_velocity side_direction = get_vertical_vector(forward)[1] side_norm = norm(side_direction[0], side_direction[1]) + 1e-6 project_on_side = (vector[0] * side_direction[0] + vector[1] * side_direction[1]) / side_norm return project_on_heading, project_on_side
def sharpbend( previous_lane: "StraightLane", following_lane_length, radius: float, angle: float, clockwise: bool = True, width: float = AbstractLane.DEFAULT_WIDTH, line_types: Tuple[LineType, LineType] = None, forbidden: bool = False, speed_limit: float = 20, priority: int = 0 ): bend_direction = 1 if clockwise else -1 center = previous_lane.position(previous_lane.length, bend_direction * radius) p_lateral = previous_lane.direction_lateral x, y = p_lateral start_phase = 0 if y == 0: start_phase = 0 if x < 0 else -np.pi elif x == 0: start_phase = np.pi / 2 if y < 0 else -np.pi / 2 else: base_angel = np.arctan(y / x) if x < 0: start_phase = base_angel elif y < 0: start_phase = np.pi + base_angel elif y > 0: start_phase = -np.pi + base_angel end_phase = start_phase + angle if not clockwise: start_phase = start_phase - np.pi end_phase = start_phase - angle bend = CircularLane( center, radius, start_phase, end_phase, clockwise, width, line_types, forbidden, speed_limit, priority ) length = 2 * radius * angle / 2 bend_end = bend.position(length, 0) next_lane_heading = get_vertical_vector(bend_end - center) nxt_dir = next_lane_heading[0] if not clockwise else next_lane_heading[1] nxt_dir = np.asarray(nxt_dir) following_lane_end = nxt_dir * following_lane_length + bend_end following_lane = StraightLane(bend_end, following_lane_end, width, line_types, forbidden, speed_limit, priority) return bend, following_lane