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 vehicle_state(self, vehicle): # update out of road info = [] lateral_to_left, lateral_to_right, = vehicle.dist_to_left_side, vehicle.dist_to_right_side total_width = float( (vehicle.routing_localization.get_current_lane_num() + 1) * vehicle.routing_localization.get_current_lane_width()) lateral_to_left /= total_width lateral_to_right /= total_width info += [ clip(lateral_to_left, 0.0, 1.0), clip(lateral_to_right, 0.0, 1.0) ] current_reference_lane = vehicle.routing_localization.current_ref_lanes[ -1] info += [ vehicle.heading_diff(current_reference_lane), # Note: speed can be negative denoting free fall. This happen when emergency brake. clip((vehicle.speed + 1) / (vehicle.max_speed + 1), 0.0, 1.0), clip((vehicle.steering / vehicle.max_steering + 1) / 2, 0.0, 1.0), clip((vehicle.last_current_action[0][0] + 1) / 2, 0.0, 1.0), clip((vehicle.last_current_action[0][1] + 1) / 2, 0.0, 1.0) ] heading_dir_last = vehicle.last_heading_dir heading_dir_now = vehicle.heading # Add more information about the road if hasattr(current_reference_lane, "heading"): info.append(clip(current_reference_lane.heading, 0.0, 1.0)) else: info.append(0.0) info.append(clip(current_reference_lane.length / DISTANCE, 0.0, 1.0)) if hasattr(current_reference_lane, "direction"): dir = current_reference_lane.direction if isinstance(dir, int): info.append(self._to_zero_and_one(dir)) info.append(0.0) elif len(dir) == 2: info.append(self._to_zero_and_one(dir[0])) info.append(self._to_zero_and_one(dir[1])) else: info.extend([0.0, 0.0]) else: info.extend([0.0, 0.0]) cos_beta = heading_dir_now.dot(heading_dir_last) / ( norm(*heading_dir_now) * norm(*heading_dir_last)) beta_diff = np.arccos(clip(cos_beta, 0.0, 1.0)) yaw_rate = beta_diff / 0.1 info.append(clip(yaw_rate, 0.0, 1.0)) long, lateral = vehicle.lane.local_coordinates(vehicle.position) info.append( clip( (lateral * 2 / vehicle.routing_localization.get_current_lane_width() + 1.0) / 2.0, 0.0, 1.0)) info.append(clip(long / DISTANCE, 0.0, 1.0)) return info
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 vehicle_state(self, vehicle): """ Wrap vehicle states to list """ # update out of road info = [] if hasattr(vehicle, "side_detector") and vehicle.side_detector.available: info += vehicle.side_detector.perceive( vehicle, vehicle.engine.physics_world.static_world).cloud_points else: lateral_to_left, lateral_to_right, = vehicle.dist_to_left_side, vehicle.dist_to_right_side total_width = float((vehicle.navigation.map.MAX_LANE_NUM + 1) * vehicle.navigation.map.MAX_LANE_WIDTH) lateral_to_left /= total_width lateral_to_right /= total_width info += [ clip(lateral_to_left, 0.0, 1.0), clip(lateral_to_right, 0.0, 1.0) ] # print("Heading Diff: ", [ # vehicle.heading_diff(current_reference_lane) # for current_reference_lane in vehicle.navigation.current_ref_lanes # ]) current_reference_lane = vehicle.navigation.current_ref_lanes[-1] info += [ vehicle.heading_diff(current_reference_lane), # Note: speed can be negative denoting free fall. This happen when emergency brake. clip((vehicle.speed + 1) / (vehicle.max_speed + 1), 0.0, 1.0), clip((vehicle.steering / vehicle.MAX_STEERING + 1) / 2, 0.0, 1.0), clip((vehicle.last_current_action[0][0] + 1) / 2, 0.0, 1.0), clip((vehicle.last_current_action[0][1] + 1) / 2, 0.0, 1.0) ] heading_dir_last = vehicle.last_heading_dir heading_dir_now = vehicle.heading cos_beta = heading_dir_now.dot(heading_dir_last) / ( norm(*heading_dir_now) * norm(*heading_dir_last)) beta_diff = np.arccos(clip(cos_beta, 0.0, 1.0)) # print(beta) yaw_rate = beta_diff / 0.1 # print(yaw_rate) info.append(clip(yaw_rate, 0.0, 1.0)) if vehicle.lane_line_detector.available: info += vehicle.lane_line_detector.perceive( vehicle, vehicle.engine.physics_world.static_world).cloud_points # else: # _, lateral = vehicle.lane.local_coordinates(vehicle.position) # info.append(clip((lateral * 2 / vehicle.navigation.map.MAX_LANE_WIDTH + 1.0) / 2.0, 0.0, 1.0)) # add vehicle length/width if self.config["random_agent_model"]: info.append(clip(vehicle.LENGTH / vehicle.MAX_LENGTH, 0.0, 1.0)) info.append(clip(vehicle.WIDTH / vehicle.MAX_WIDTH, 0.0, 1.0)) return info
def vehicle_state(vehicle): """ Wrap vehicle states to list """ # update out of road info = [] if hasattr(vehicle, "side_detector") and vehicle.side_detector is not None: info += vehicle.side_detector.get_cloud_points() else: lateral_to_left, lateral_to_right, = vehicle.dist_to_left_side, vehicle.dist_to_right_side total_width = float( (vehicle.routing_localization.get_current_lane_num() + 1) * vehicle.routing_localization.get_current_lane_width()) lateral_to_left /= total_width lateral_to_right /= total_width info += [ clip(lateral_to_left, 0.0, 1.0), clip(lateral_to_right, 0.0, 1.0) ] # print("Heading Diff: ", [ # vehicle.heading_diff(current_reference_lane) # for current_reference_lane in vehicle.routing_localization.current_ref_lanes # ]) current_reference_lane = vehicle.routing_localization.current_ref_lanes[ -1] info += [ vehicle.heading_diff(current_reference_lane), # Note: speed can be negative denoting free fall. This happen when emergency brake. clip((vehicle.speed + 1) / (vehicle.max_speed + 1), 0.0, 1.0), clip((vehicle.steering / vehicle.max_steering + 1) / 2, 0.0, 1.0), clip((vehicle.last_current_action[0][0] + 1) / 2, 0.0, 1.0), clip((vehicle.last_current_action[0][1] + 1) / 2, 0.0, 1.0) ] heading_dir_last = vehicle.last_heading_dir heading_dir_now = vehicle.heading cos_beta = heading_dir_now.dot(heading_dir_last) / ( norm(*heading_dir_now) * norm(*heading_dir_last)) beta_diff = np.arccos(clip(cos_beta, 0.0, 1.0)) # print(beta) yaw_rate = beta_diff / 0.1 # print(yaw_rate) info.append(clip(yaw_rate, 0.0, 1.0)) if vehicle.lane_line_detector is not None: info += vehicle.lane_line_detector.get_cloud_points() else: _, lateral = vehicle.lane.local_coordinates(vehicle.position) info.append( clip((lateral * 2 / vehicle.routing_localization.get_current_lane_width() + 1.0) / 2.0, 0.0, 1.0)) return info
def ray_localization(heading: tuple, position: tuple, engine: EngineCore, return_all_result=False) -> Union[List[Tuple], Tuple]: """ Get the index of the lane closest to a physx_world position. Only used when smoething is on lane ! Otherwise fall back to use get_closest_lane() :param heading: heading to help filter lanes :param position: a physx_world position [m]. :param engine: PGWorld class :param return_all_result: return a list instead of the lane with min L1 distance :return: list(closest lane) or closest lane. """ results = engine.physics_world.static_world.rayTestAll( panda_position(position, 1.0), panda_position(position, -1.0)) lane_index_dist = [] if results.hasHits(): for res in results.getHits(): if res.getNode().getName() == BodyName.Lane: lane = get_object_from_node(res.getNode()) long, _ = lane.local_coordinates(position) lane_heading = lane.heading_at(long) # dir = np.array([math.cos(lane_heading), math.sin(lane_heading)]) # dot_result = dir.dot(heading) dot_result = math.cos(lane_heading) * heading[0] + math.sin( lane_heading) * heading[1] cosangle = dot_result / ( norm(math.cos(lane_heading), math.sin(lane_heading)) * norm(heading[0], heading[1])) if cosangle > 0: lane_index_dist.append( (lane, lane.index, lane.distance(position))) if return_all_result: ret = [] if len(lane_index_dist) > 0: for lane, index, dist in lane_index_dist: ret.append((lane, index, dist)) sorted(ret, key=lambda k: k[2]) return ret else: if len(lane_index_dist) > 0: ret_index = np.argmin([d for _, _, d in lane_index_dist]) lane, index, dist = lane_index_dist[ret_index] else: lane, index, dist = None, None, None return lane, index
def speed(self): """ km/h """ velocity = self.chassis_np.node().get_linear_velocity() speed = norm(velocity[0], velocity[1]) * 3.6 return clip(speed, 0.0, 100000.0)
def _add_box_body(self, lane_start, lane_end, middle, parent_np: NodePath, line_type, line_color): length = norm(lane_end[0] - lane_start[0], lane_end[1] - lane_start[1]) if LineType.prohibit(line_type): node_name = BodyName.White_continuous_line if line_color == LineColor.GREY else BodyName.Yellow_continuous_line else: node_name = BodyName.Broken_line body_node = BulletGhostNode(node_name) body_node.set_active(False) body_node.setKinematic(False) body_node.setStatic(True) body_np = parent_np.attachNewNode(body_node) shape = BulletBoxShape( Vec3(length / 2, Block.LANE_LINE_WIDTH / 2, Block.LANE_LINE_GHOST_HEIGHT)) body_np.node().addShape(shape) mask = Block.CONTINUOUS_COLLISION_MASK if line_type != LineType.BROKEN else Block.BROKEN_COLLISION_MASK body_np.node().setIntoCollideMask(BitMask32.bit(mask)) self.static_nodes.append(body_np.node()) body_np.setPos(panda_position(middle, Block.LANE_LINE_GHOST_HEIGHT / 2)) direction_v = lane_end - lane_start theta = -numpy.arctan2(direction_v[1], direction_v[0]) body_np.setQuat( LQuaternionf(numpy.cos(theta / 2), 0, 0, numpy.sin(theta / 2)))
def _add_side_walk2bullet(self, lane_start, lane_end, middle, radius=0.0, direction=0): length = norm(lane_end[0] - lane_start[0], lane_end[1] - lane_start[1]) body_node = BulletRigidBodyNode(BodyName.Side_walk) body_node.setActive(False) body_node.setKinematic(False) body_node.setStatic(True) side_np = self.side_walk_node_path.attachNewNode(body_node) shape = BulletBoxShape(Vec3(1 / 2, 1 / 2, 1 / 2)) body_node.addShape(shape) body_node.setIntoCollideMask(BitMask32.bit(Block.LANE_LINE_COLLISION_MASK)) self.dynamic_nodes.append(body_node) if radius == 0: factor = 1 else: if direction == 1: factor = (1 - self.SIDE_WALK_LINE_DIST / radius) else: factor = (1 + self.SIDE_WALK_WIDTH / radius) * (1 + self.SIDE_WALK_LINE_DIST / radius) direction_v = lane_end - lane_start vertical_v = (-direction_v[1], direction_v[0]) / numpy.linalg.norm(direction_v) middle += vertical_v * (self.SIDE_WALK_WIDTH / 2 + self.SIDE_WALK_LINE_DIST) side_np.setPos(panda_position(middle, 0)) theta = -numpy.arctan2(direction_v[1], direction_v[0]) side_np.setQuat(LQuaternionf(numpy.cos(theta / 2), 0, 0, numpy.sin(theta / 2))) side_np.setScale( length * factor, self.SIDE_WALK_WIDTH, self.SIDE_WALK_THICKNESS * (1 + 0.1 * numpy.random.rand()) ) if self.render: side_np.setTexture(self.ts_color, self.side_texture) self.side_walk.instanceTo(side_np)
def speed(self): """ km/h """ velocity = self.body.get_linear_velocity() speed = norm(velocity[0], velocity[1]) * 3.6 return clip(speed, 0.0, 100000.0)
def __init__(self, start: Vector, end: Vector, width: float = AbstractLane.DEFAULT_WIDTH, line_types: Tuple[LineType, LineType] = (LineType.STRIPED, LineType.STRIPED), forbidden: bool = False, speed_limit: float = 20, priority: int = 0) -> None: """ New straight lane. :param start: the lane starting position [m] :param end: the lane ending position [m] :param width: the lane width [m] :param line_types: the type of lines on both sides of the lane :param forbidden: is changing to this lane forbidden :param priority: priority level of the lane, for determining who has right of way """ self.start = np.array(start) self.end = np.array(end) self.width = width self.line_types = line_types or [LineType.STRIPED, LineType.STRIPED] self.forbidden = forbidden self.priority = priority self.speed_limit = speed_limit self.length = norm((self.end - self.start)[0], (self.end - self.start)[1]) self.heading = math.atan2(self.end[1] - self.start[1], self.end[0] - self.start[0]) self.direction = (self.end - self.start) / self.length self.direction_lateral = np.array( [-self.direction[1], self.direction[0]])
def update_properties(self): self.length = norm((self.end - self.start)[0], (self.end - self.start)[1]) self.heading = math.atan2(self.end[1] - self.start[1], self.end[0] - self.start[0]) self.direction = (self.end - self.start) / self.length self.direction_lateral = np.array( [-self.direction[1], self.direction[0]])
def local_coordinates(self, position: Tuple[float, float]) -> Tuple[float, float]: delta_x = position[0] - self.center[0] delta_y = position[1] - self.center[1] phi = math.atan2(delta_y, delta_x) phi = self.start_phase + wrap_to_pi(phi - self.start_phase) r = norm(delta_x, delta_y) longitudinal = self.direction * (phi - self.start_phase) * self.radius lateral = self.direction * (self.radius - r) return longitudinal, lateral
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 _add_lane_line2bullet(self, lane_start, lane_end, middle, parent_np: NodePath, color: Vec4, line_type: LineType, straight_stripe=False): length = norm(lane_end[0] - lane_start[0], lane_end[1] - lane_start[1]) if length <= 0: return if LineType.prohibit(line_type): node_name = BodyName.White_continuous_line if color == LineColor.GREY else BodyName.Yellow_continuous_line else: node_name = BodyName.Broken_line # add bullet body for it if straight_stripe: body_np = parent_np.attachNewNode(node_name) else: body_node = BulletGhostNode(node_name) body_node.set_active(False) body_node.setKinematic(False) body_node.setStatic(True) body_np = parent_np.attachNewNode(body_node) # its scale will change by setScale body_height = DrivableAreaProperty.LANE_LINE_GHOST_HEIGHT shape = BulletBoxShape( Vec3(length / 2 if line_type != LineType.BROKEN else length, DrivableAreaProperty.LANE_LINE_WIDTH / 2, body_height)) body_np.node().addShape(shape) mask = DrivableAreaProperty.CONTINUOUS_COLLISION_MASK if line_type != LineType.BROKEN else DrivableAreaProperty.BROKEN_COLLISION_MASK body_np.node().setIntoCollideMask(mask) self.static_nodes.append(body_np.node()) # position and heading body_np.setPos( panda_position(middle, DrivableAreaProperty.LANE_LINE_GHOST_HEIGHT / 2)) direction_v = lane_end - lane_start # theta = -numpy.arctan2(direction_v[1], direction_v[0]) theta = -math.atan2(direction_v[1], direction_v[0]) body_np.setQuat( LQuaternionf(math.cos(theta / 2), 0, 0, math.sin(theta / 2))) if self.render: # For visualization lane_line = self.loader.loadModel( AssetLoader.file_path("models", "box.bam")) lane_line.setScale(length, DrivableAreaProperty.LANE_LINE_WIDTH, DrivableAreaProperty.LANE_LINE_THICKNESS) lane_line.setPos( Vec3(0, 0 - DrivableAreaProperty.LANE_LINE_GHOST_HEIGHT / 2)) lane_line.reparentTo(body_np) body_np.set_color(color)
def cull_distant_traffic_vehicles(cls, vehicles: list, pos, pg_world: PGWorld): # Cull distant vehicles for v in vehicles: v_p = v.position if norm(v_p[0] - pos[0], v_p[1] - v_p[1]) < cls.LOD_VEHICLE_VIS_DIST: if not v.node_path.hasParent(): v.node_path.reparentTo(pg_world.pbr_worldNP) else: if v.node_path.hasParent(): v.node_path.detachNode() if norm(v_p[0] - pos[0], v_p[1] - v_p[1]) < cls.LOD_VEHICLE_PHYSICS_DIST: v.dynamic_nodes.attach_to_physics_world( pg_world.physics_world.dynamic_world) else: v.dynamic_nodes.detach_from_physics_world( pg_world.physics_world.dynamic_world)
def _update_energy_consumption(self): """ The calculation method is from https://www.researchgate.net/publication/262182035_Reduction_of_Fuel_Consumption_and_Exhaust_Pollutant_Using_Intelligent_Transport_System default: 3rd gear, try to use ae^bx to fit it, dp: (90, 8), (130, 12) :return: None """ distance = norm(*(self.last_position - self.position)) / 1000 # km step_energy = 3.25 * math.pow(np.e, 0.01 * self.speed) * distance / 100 # step_energy is in Liter, we return mL step_energy = step_energy * 1000 self.energy_consumption += step_energy # L/100 km return step_energy, self.energy_consumption
def _add_lane_line2bullet( self, lane_start, lane_end, middle, parent_np: NodePath, color: Vec4, line_type: LineType, straight_stripe=False ): length = norm(lane_end[0] - lane_start[0], lane_end[1] - lane_start[1]) if length <= 0: return if LineType.prohibit(line_type): node_name = BodyName.Continuous_line else: node_name = BodyName.Stripped_line # add bullet body for it if straight_stripe: body_np = parent_np.attachNewNode(node_name) else: scale = 2 if line_type == LineType.STRIPED else 1 body_node = BulletRigidBodyNode(node_name) body_node.setActive(False) body_node.setKinematic(False) body_node.setStatic(True) body_np = parent_np.attachNewNode(body_node) shape = BulletBoxShape(Vec3(scale / 2, Block.LANE_LINE_WIDTH / 2, Block.LANE_LINE_THICKNESS)) body_np.node().addShape(shape) body_np.node().setIntoCollideMask(BitMask32.bit(Block.LANE_LINE_COLLISION_MASK)) self.dynamic_nodes.append(body_np.node()) # position and heading body_np.setPos(panda_position(middle, 0)) direction_v = lane_end - lane_start theta = -numpy.arctan2(direction_v[1], direction_v[0]) body_np.setQuat(LQuaternionf(numpy.cos(theta / 2), 0, 0, numpy.sin(theta / 2))) if self.render: # For visualization lane_line = self.loader.loadModel(AssetLoader.file_path("models", "box.bam")) lane_line.getChildren().reparentTo(body_np) body_np.setScale(length, Block.LANE_LINE_WIDTH, Block.LANE_LINE_THICKNESS) body_np.set_color(color)
def _add_box_body(self, lane_start, lane_end, middle, parent_np: NodePath, line_type): length = norm(lane_end[0] - lane_start[0], lane_end[1] - lane_start[1]) if LineType.prohibit(line_type): node_name = BodyName.Continuous_line else: node_name = BodyName.Stripped_line body_node = BulletRigidBodyNode(node_name) body_node.setActive(False) body_node.setKinematic(False) body_node.setStatic(True) body_np = parent_np.attachNewNode(body_node) shape = BulletBoxShape(Vec3(length / 2, Block.LANE_LINE_WIDTH / 2, Block.LANE_LINE_THICKNESS)) body_np.node().addShape(shape) body_np.node().setIntoCollideMask(BitMask32.bit(Block.LANE_LINE_COLLISION_MASK)) self.dynamic_nodes.append(body_np.node()) body_np.setPos(panda_position(middle, 0)) direction_v = lane_end - lane_start theta = -numpy.arctan2(direction_v[1], direction_v[0]) body_np.setQuat(LQuaternionf(numpy.cos(theta / 2), 0, 0, numpy.sin(theta / 2)))
def get_surrounding_vehicles_info(self, ego_vehicle, num_others: int = 4): from pgdrive.utils.math_utils import norm, clip surrounding_vehicles = list(self.get_surrounding_vehicles()) surrounding_vehicles.sort( key=lambda v: norm(ego_vehicle.position[0] - v.position[0], ego_vehicle.position[1] - v.position[1]) ) surrounding_vehicles += [None] * num_others res = [] for vehicle in surrounding_vehicles[:num_others]: if vehicle is not None: # assert isinstance(vehicle, IDMVehicle or Base), "Now PGDrive Doesn't support other vehicle type" relative_position = ego_vehicle.projection(vehicle.position - ego_vehicle.position) # It is possible that the centroid of other vehicle is too far away from ego but lidar shed on it. # So the distance may greater than perceive distance. res.append(clip((relative_position[0] / self.perceive_distance + 1) / 2, 0.0, 1.0)) res.append(clip((relative_position[1] / self.perceive_distance + 1) / 2, 0.0, 1.0)) relative_velocity = ego_vehicle.projection(vehicle.velocity - ego_vehicle.velocity) res.append(clip((relative_velocity[0] / ego_vehicle.max_speed + 1) / 2, 0.0, 1.0)) res.append(clip((relative_velocity[1] / ego_vehicle.max_speed + 1) / 2, 0.0, 1.0)) else: res += [0.0] * 4 return res
def overwritten_get_surrounding_vehicles_info(self, lidar, ego_vehicle, num_others: int = 4): # surrounding_vehicles = list(lidar.get_surrounding_vehicles()) surrounding_vehicles = list( self._env.scene_manager.traffic_manager.vehicles)[1:] surrounding_vehicles.sort( key=lambda v: norm(ego_vehicle.position[0] - v.position[0], ego_vehicle.position[1] - v.position[1])) # dis = [(str(v), norm(ego_vehicle.position[0] - v.position[0], ego_vehicle.position[1] - v.position[1])) # for v in surrounding_vehicles] surrounding_vehicles += [None] * num_others res = [] for vehicle in surrounding_vehicles[:num_others]: if vehicle is not None: relative_position = ego_vehicle.projection( vehicle.position - ego_vehicle.position) dist = norm(relative_position[0], relative_position[1]) if dist > DISTANCE: if self.config["use_extra_state"]: res += [0.0] * self._traffic_vehicle_state_dim else: res += [0.0] * self._traffic_vehicle_state_dim_wo_extra continue relative_velocity = ego_vehicle.projection( vehicle.velocity - ego_vehicle.velocity) res.append(clip(dist / DISTANCE, 0.0, 1.0)) res.append( clip( norm(relative_velocity[0], relative_velocity[1]) / ego_vehicle.max_speed, 0.0, 1.0)) res.append( clip((relative_position[0] / DISTANCE + 1) / 2, 0.0, 1.0)) res.append( clip((relative_position[1] / DISTANCE + 1) / 2, 0.0, 1.0)) res.append( clip( (relative_velocity[0] / ego_vehicle.max_speed + 1) / 2, 0.0, 1.0)) res.append( clip( (relative_velocity[1] / ego_vehicle.max_speed + 1) / 2, 0.0, 1.0)) if self.config["use_extra_state"]: res.extend(self.traffic_vehicle_state(vehicle)) else: if self.config["use_extra_state"]: res += [0.0] * self._traffic_vehicle_state_dim else: res += [0.0] * self._traffic_vehicle_state_dim_wo_extra # p1 = [] # p2 = [] # for vehicle in surrounding_vehicles[:num_others]: # if vehicle is not None: # relative_position = ego_vehicle.projection(vehicle.position - ego_vehicle.position) # relative_velocity = ego_vehicle.projection(vehicle.velocity - ego_vehicle.velocity) # p1.append(relative_position) # p2.append(relative_velocity) # print("Detected Others Position: {}, Velocity: {}".format(p1, p2)) return res
def update_navigation_localization(self, ego_vehicle): position = ego_vehicle.position lane, lane_index = ray_localization(position, ego_vehicle.pg_world) if lane is None: lane, lane_index = ego_vehicle.lane, ego_vehicle.lane_index if self.FORCE_CALCULATE: lane_index, _ = self.map.road_network.get_closest_lane_index( position) lane = self.map.road_network.get_lane(lane_index) self._update_target_checkpoints(lane_index) target_road_1_start = self.checkpoints[ self.target_checkpoints_index[0]] target_road_1_end = self.checkpoints[self.target_checkpoints_index[0] + 1] target_road_2_start = self.checkpoints[ self.target_checkpoints_index[1]] target_road_2_end = self.checkpoints[self.target_checkpoints_index[1] + 1] target_lanes_1 = self.map.road_network.graph[target_road_1_start][ target_road_1_end] target_lanes_2 = self.map.road_network.graph[target_road_2_start][ target_road_2_end] res = [] self.current_ref_lanes = target_lanes_1 ckpts = [] lanes_heading = [] for lanes_id, lanes in enumerate([target_lanes_1, target_lanes_2]): ref_lane = lanes[0] later_middle = (float(self.map.lane_num) / 2 - 0.5) * self.map.lane_width if isinstance(ref_lane, CircularLane) and ref_lane.direction == 1: ref_lane = lanes[-1] later_middle *= -1 check_point = ref_lane.position(ref_lane.length, later_middle) if lanes_id == 0: # calculate ego v lane heading lanes_heading.append( ref_lane.heading_at( ref_lane.local_coordinates(ego_vehicle.position)[0])) else: lanes_heading.append( ref_lane.heading_at( min(self.PRE_NOTIFY_DIST, ref_lane.length))) ckpts.append(check_point) dir_vec = check_point - ego_vehicle.position dir_norm = norm(dir_vec[0], dir_vec[1]) if dir_norm > self.NAVI_POINT_DIST: dir_vec = dir_vec / dir_norm * self.NAVI_POINT_DIST proj_heading, proj_side = ego_vehicle.projection(dir_vec) bendradius = 0.0 dir = 0.0 angle = 0.0 if isinstance(ref_lane, CircularLane): bendradius = ref_lane.radius / ( BlockParameterSpace.CURVE[Parameter.radius].max + self.map.lane_num * self.map.lane_width) dir = ref_lane.direction if dir == 1: angle = ref_lane.end_phase - ref_lane.start_phase elif dir == -1: angle = ref_lane.start_phase - ref_lane.end_phase res += [ clip((proj_heading / self.NAVI_POINT_DIST + 1) / 2, 0.0, 1.0), clip((proj_side / self.NAVI_POINT_DIST + 1) / 2, 0.0, 1.0), clip(bendradius, 0.0, 1.0), clip((dir + 1) / 2, 0.0, 1.0), clip((np.rad2deg(angle) / BlockParameterSpace.CURVE[Parameter.angle].max + 1) / 2, 0.0, 1.0) ] if self.show_navi_point: pos_of_goal = ckpts[0] self.goal_node_path.setPos(pos_of_goal[0], -pos_of_goal[1], 1.8) self.goal_node_path.setH(self.goal_node_path.getH() + 3) self.update_navi_arrow(lanes_heading) self.navi_info = res return lane, lane_index
def _add_lane(self, lane: AbstractLane, lane_id: int, colors: List[Vec4]): parent_np = self.lane_line_node_path lane_width = lane.width_at(0) for k, i in enumerate([-1, 1]): line_color = colors[k] if lane.line_types[k] == LineType.NONE or (lane_id != 0 and k == 0): if isinstance(lane, StraightLane): continue elif isinstance( lane, CircularLane) and lane.radius != lane_width / 2: # for ramp render continue if lane.line_types[k] == LineType.CONTINUOUS or lane.line_types[ k] == LineType.SIDE: if isinstance(lane, StraightLane): lane_start = lane.position(0, i * lane_width / 2) lane_end = lane.position(lane.length, i * lane_width / 2) middle = lane.position(lane.length / 2, i * lane_width / 2) self._add_lane_line2bullet(lane_start, lane_end, middle, parent_np, line_color, lane.line_types[k]) elif isinstance(lane, CircularLane): segment_num = int(lane.length / Block.CIRCULAR_SEGMENT_LENGTH) for segment in range(segment_num): lane_start = lane.position( segment * Block.CIRCULAR_SEGMENT_LENGTH, i * lane_width / 2) lane_end = lane.position( (segment + 1) * Block.CIRCULAR_SEGMENT_LENGTH, i * lane_width / 2) middle = (lane_start + lane_end) / 2 self._add_lane_line2bullet(lane_start, lane_end, middle, parent_np, line_color, lane.line_types[k]) # for last part lane_start = lane.position( segment_num * Block.CIRCULAR_SEGMENT_LENGTH, i * lane_width / 2) lane_end = lane.position(lane.length, i * lane_width / 2) middle = (lane_start + lane_end) / 2 self._add_lane_line2bullet(lane_start, lane_end, middle, parent_np, line_color, lane.line_types[k]) if lane.line_types[k] == LineType.SIDE: radius = lane.radius if isinstance(lane, CircularLane) else 0.0 segment_num = int(lane.length / Block.SIDEWALK_LENGTH) for segment in range(segment_num): lane_start = lane.position( segment * Block.SIDEWALK_LENGTH, i * lane_width / 2) lane_end = lane.position( (segment + 1) * Block.SIDEWALK_LENGTH, i * lane_width / 2) middle = (lane_start + lane_end) / 2 self._add_sidewalk2bullet(lane_start, lane_end, middle, radius, lane.direction) # for last part lane_start = lane.position( segment_num * Block.SIDEWALK_LENGTH, i * lane_width / 2) lane_end = lane.position(lane.length, i * lane_width / 2) middle = (lane_start + lane_end) / 2 if norm(lane_start[0] - lane_end[0], lane_start[1] - lane_end[1]) > 1e-1: self._add_sidewalk2bullet(lane_start, lane_end, middle, radius, lane.direction) elif lane.line_types[k] == LineType.BROKEN: straight = True if isinstance(lane, StraightLane) else False segment_num = int(lane.length / (2 * Block.STRIPE_LENGTH)) for segment in range(segment_num): lane_start = lane.position( segment * Block.STRIPE_LENGTH * 2, i * lane_width / 2) lane_end = lane.position( segment * Block.STRIPE_LENGTH * 2 + Block.STRIPE_LENGTH, i * lane_width / 2) middle = lane.position( segment * Block.STRIPE_LENGTH * 2 + Block.STRIPE_LENGTH / 2, i * lane_width / 2) self._add_lane_line2bullet(lane_start, lane_end, middle, parent_np, line_color, lane.line_types[k], straight) lane_start = lane.position( segment_num * Block.STRIPE_LENGTH * 2, i * lane_width / 2) lane_end = lane.position(lane.length + Block.STRIPE_LENGTH, i * lane_width / 2) middle = (lane_end[0] + lane_start[0]) / 2, (lane_end[1] + lane_start[1]) / 2 if not straight: self._add_lane_line2bullet(lane_start, lane_end, middle, parent_np, line_color, lane.line_types[k], straight) if straight: lane_start = lane.position(0, i * lane_width / 2) lane_end = lane.position(lane.length, i * lane_width / 2) middle = lane.position(lane.length / 2, i * lane_width / 2) self._add_box_body(lane_start, lane_end, middle, parent_np, lane.line_types[k], line_color)
def all_distance_greater_than(distance, poses, target_pos): v_p = target_pos for pos in poses: if norm(v_p[0] - pos[0], v_p[1] - v_p[1]) < distance: return False return True
def _add_pgdrive_lanes(self, lane, lane_id, lane_width, colors, parent_np): # for pgdrive structure for k, i in enumerate([-1, 1]): line_color = colors[k] if lane.line_types[k] == LineType.NONE or (lane_id != 0 and k == 0): if isinstance(lane, StraightLane): continue elif isinstance( lane, CircularLane) and lane.radius != lane_width / 2: # for ramp render continue if lane.line_types[k] == LineType.CONTINUOUS or lane.line_types[ k] == LineType.SIDE: if isinstance(lane, StraightLane): lane_start = lane.position(0, i * lane_width / 2) lane_end = lane.position(lane.length, i * lane_width / 2) middle = lane.position(lane.length / 2, i * lane_width / 2) self._add_lane_line2bullet(lane_start, lane_end, middle, parent_np, line_color, lane.line_types[k]) elif isinstance(lane, CircularLane): segment_num = int( lane.length / DrivableAreaProperty.CIRCULAR_SEGMENT_LENGTH) for segment in range(segment_num): lane_start = lane.position( segment * DrivableAreaProperty.CIRCULAR_SEGMENT_LENGTH, i * lane_width / 2) lane_end = lane.position( (segment + 1) * DrivableAreaProperty.CIRCULAR_SEGMENT_LENGTH, i * lane_width / 2) middle = (lane_start + lane_end) / 2 self._add_lane_line2bullet(lane_start, lane_end, middle, parent_np, line_color, lane.line_types[k]) # for last part lane_start = lane.position( segment_num * DrivableAreaProperty.CIRCULAR_SEGMENT_LENGTH, i * lane_width / 2) lane_end = lane.position(lane.length, i * lane_width / 2) middle = (lane_start + lane_end) / 2 self._add_lane_line2bullet(lane_start, lane_end, middle, parent_np, line_color, lane.line_types[k]) if lane.line_types[k] == LineType.SIDE: radius = lane.radius if isinstance(lane, CircularLane) else 0.0 segment_num = int(lane.length / DrivableAreaProperty.SIDEWALK_LENGTH) for segment in range(segment_num): lane_start = lane.position( segment * DrivableAreaProperty.SIDEWALK_LENGTH, i * lane_width / 2) lane_end = lane.position( (segment + 1) * DrivableAreaProperty.SIDEWALK_LENGTH, i * lane_width / 2) middle = (lane_start + lane_end) / 2 self._add_sidewalk2bullet(lane_start, lane_end, middle, radius, lane.direction) # for last part lane_start = lane.position( segment_num * DrivableAreaProperty.SIDEWALK_LENGTH, i * lane_width / 2) lane_end = lane.position(lane.length, i * lane_width / 2) middle = (lane_start + lane_end) / 2 if norm(lane_start[0] - lane_end[0], lane_start[1] - lane_end[1]) > 1e-1: self._add_sidewalk2bullet(lane_start, lane_end, middle, radius, lane.direction) elif lane.line_types[k] == LineType.BROKEN: straight = True if isinstance(lane, StraightLane) else False segment_num = int(lane.length / (2 * DrivableAreaProperty.STRIPE_LENGTH)) for segment in range(segment_num): lane_start = lane.position( segment * DrivableAreaProperty.STRIPE_LENGTH * 2, i * lane_width / 2) lane_end = lane.position( segment * DrivableAreaProperty.STRIPE_LENGTH * 2 + DrivableAreaProperty.STRIPE_LENGTH, i * lane_width / 2) middle = lane.position( segment * DrivableAreaProperty.STRIPE_LENGTH * 2 + DrivableAreaProperty.STRIPE_LENGTH / 2, i * lane_width / 2) self._add_lane_line2bullet(lane_start, lane_end, middle, parent_np, line_color, lane.line_types[k], straight) lane_start = lane.position( segment_num * DrivableAreaProperty.STRIPE_LENGTH * 2, i * lane_width / 2) lane_end = lane.position( lane.length + DrivableAreaProperty.STRIPE_LENGTH, i * lane_width / 2) middle = (lane_end[0] + lane_start[0]) / 2, (lane_end[1] + lane_start[1]) / 2 if not straight: self._add_lane_line2bullet(lane_start, lane_end, middle, parent_np, line_color, lane.line_types[k], straight) if straight: lane_start = lane.position(0, i * lane_width / 2) lane_end = lane.position(lane.length, i * lane_width / 2) middle = lane.position(lane.length / 2, i * lane_width / 2) self._add_box_body(lane_start, lane_end, middle, parent_np, lane.line_types[k], line_color)