class Bottleneck(PGBlock): """ This block is used to change thr lane num """ ID = None SOCKET_NUM = 1 PARAMETER_SPACE = ParameterSpace(BlockParameterSpace.BOTTLENECK_PARAMETER) # property of bottleneck BOTTLENECK_LEN = None
class DefaultVehicle(BaseVehicle): PARAMETER_SPACE = ParameterSpace(VehicleParameterSpace.DEFAULT_VEHICLE) LENGTH = 4.51 WIDTH = 1.852 HEIGHT = 1.19 TIRE_RADIUS = 0.313 MASS = 1100 LATERAL_TIRE_TO_CENTER = 0.815 FRONT_WHEELBASE = 1.05234 REAR_WHEELBASE = 1.4166 path = [['ferra/scene.gltf', (factor, factor, factor), (0, 0.0, 0.), 0]]
class LVehicle(BaseVehicle): PARAMETER_SPACE = ParameterSpace(VehicleParameterSpace.L_VEHICLE) LENGTH = 4.5 WIDTH = 1.86 HEIGHT = 1.85 TIRE_RADIUS = 0.39 REAR_WHEELBASE = 1.10751 FRONT_WHEELBASE = 1.391 LATERAL_TIRE_TO_CENTER = 0.75 MASS = 1300 path = [ ['new/lada/scene.gltf', (factor, factor, factor), (0, -0.25, 0.07), 0], ]
class MVehicle(BaseVehicle): PARAMETER_SPACE = ParameterSpace(VehicleParameterSpace.M_VEHICLE) LENGTH = 4.4 WIDTH = 1.85 HEIGHT = 1.37 TIRE_RADIUS = 0.39 REAR_WHEELBASE = 1.203 FRONT_WHEELBASE = 1.285 LATERAL_TIRE_TO_CENTER = 0.803 MASS = 1200 path = [ ['new/130/scene.gltf', (factor, factor, factor), (0, -0.05, 0.07), 0], ]
class XLVehicle(BaseVehicle): PARAMETER_SPACE = ParameterSpace(VehicleParameterSpace.XL_VEHICLE) LENGTH = 5.8 WIDTH = 2.3 HEIGHT = 2.8 TIRE_RADIUS = 0.37 REAR_WHEELBASE = 1.075 FRONT_WHEELBASE = 1.726 LATERAL_TIRE_TO_CENTER = 0.831 CHASSIS_TO_WHEEL_AXIS = 0.3 MASS = 1600 path = [[ 'new/truck/scene.gltf', (factor, factor, factor), (0, 0.3, 0.04), 0 ]]
class SVehicle(BaseVehicle): PARAMETER_SPACE = ParameterSpace(VehicleParameterSpace.S_VEHICLE) LENGTH = 4.25 WIDTH = 1.7 HEIGHT = 1.7 LATERAL_TIRE_TO_CENTER = 0.7 FRONT_WHEELBASE = 1.4126 REAR_WHEELBASE = 1.07 TIRE_RADIUS = 0.376 MASS = 800 path = [ [ 'new/beetle/scene.gltf', (factor, factor, factor), (0, -0.2, 0.03), 0 ], ]
class Straight(PGBlock): """ Straight Road ---------------------------------------- ---------------------------------------- ---------------------------------------- """ ID = "S" SOCKET_NUM = 1 PARAMETER_SPACE = ParameterSpace(BlockParameterSpace.STRAIGHT) def _try_plug_into_previous_block(self) -> bool: self.set_part_idx( 0) # only one part in simple block like straight, and curve para = self.get_config() length = para[Parameter.length] basic_lane = self.positive_basic_lane assert isinstance( basic_lane, StraightLane), "Straight road can only connect straight type" new_lane = ExtendStraightLane(basic_lane, length, [LineType.BROKEN, LineType.SIDE]) start = self.pre_block_socket.positive_road.end_node end = self.add_road_node() socket = Road(start, end) _socket = -socket # create positive road no_cross = CreateRoadFrom( new_lane, self.positive_lane_num, socket, self.block_network, self._global_network, ignore_intersection_checking=self.ignore_intersection_checking) # create negative road no_cross = CreateAdverseRoad(socket, self.block_network, self._global_network, ignore_intersection_checking=self. ignore_intersection_checking) and no_cross self.add_sockets(PGBlockSocket(socket, _socket)) return no_cross
class Ramp(PGBlock): """ InRamp OutRamp start ----------- end ------------------ start ----------- end ------------------ start ----------- end ------------------ start ----------- end ------------------ (start ----------- end)[----------------] start ----------- end [----------------] end -----------------} (start ---------- {end) // \\ { ---------------// \\---------------} """ PARAMETER_SPACE = ParameterSpace(BlockParameterSpace.RAMP_PARAMETER) SOCKET_NUM = 1 # can be added to parameter space in the future RADIUS = 40 # meters ANGLE = 10 # degree LANE_TYPE = (LineType.CONTINUOUS, LineType.CONTINUOUS) SPEED_LIMIT = 12 # 12 m/s ~= 40 km/h CONNECT_PART_LEN = 20 RAMP_LEN = 15
class Fork(Ramp): """ Similar to Ramp """ PARAMETER_SPACE = ParameterSpace(BlockParameterSpace.FORK_PARAMETER)
class ParkingLot(PGBlock): """ Parking Lot """ ID = "P" PARAMETER_SPACE = ParameterSpace(BlockParameterSpace.PARKING_LOT_PARAMETER) ANGLE = np.deg2rad(90) SOCKET_LENGTH = 4 # m SOCKET_NUM = 1 def _try_plug_into_previous_block(self) -> bool: self.spawn_roads = [] self.dest_roads = [] no_cross = True para = self.get_config() assert self.positive_lane_num == 1, "Lane number of previous block must be 1 in each direction" self.parking_space_length = para[Parameter.length] self.parking_space_width = self.lane_width parking_space_num = para[Parameter.one_side_vehicle_num] # parking_space_num = 10 radius = para[Parameter.radius] main_straight_road_length = 2 * radius + (parking_space_num - 1) * self.parking_space_width main_lane = ExtendStraightLane(self.positive_lanes[0], main_straight_road_length, [LineType.BROKEN, LineType.NONE]) road = Road(self.pre_block_socket.positive_road.end_node, self.road_node(0, 0)) # main straight part no_cross = CreateRoadFrom(main_lane, self.positive_lane_num, road, self.block_network, self._global_network, center_line_type=LineType.BROKEN, inner_lane_line_type=LineType.BROKEN, side_lane_line_type=LineType.NONE, center_line_color=LineColor.GREY, ignore_intersection_checking=self. ignore_intersection_checking) and no_cross no_cross = CreateAdverseRoad(road, self.block_network, self._global_network, center_line_type=LineType.BROKEN, inner_lane_line_type=LineType.BROKEN, side_lane_line_type=LineType.NONE, center_line_color=LineColor.GREY, ignore_intersection_checking=self. ignore_intersection_checking) and no_cross # socket part parking_lot_out_lane = ExtendStraightLane( main_lane, self.SOCKET_LENGTH, [LineType.BROKEN, LineType.NONE]) parking_lot_out_road = Road(self.road_node(0, 0), self.road_node(0, 1)) # out socket part no_cross = CreateRoadFrom(parking_lot_out_lane, self.positive_lane_num, parking_lot_out_road, self.block_network, self._global_network, center_line_type=LineType.BROKEN, inner_lane_line_type=LineType.BROKEN, side_lane_line_type=LineType.SIDE, ignore_intersection_checking=self. ignore_intersection_checking) and no_cross no_cross = CreateAdverseRoad(parking_lot_out_road, self.block_network, self._global_network, center_line_type=LineType.BROKEN, inner_lane_line_type=LineType.BROKEN, side_lane_line_type=LineType.SIDE, ignore_intersection_checking=self. ignore_intersection_checking) and no_cross socket = self.create_socket_from_positive_road(parking_lot_out_road) self.add_sockets(socket) # add parking space for i in range(int(parking_space_num)): no_cross = self._add_one_parking_space( copy.copy(self.get_socket_list()[0]).get_socket_in_reverse(), self.pre_block_socket.get_socket_in_reverse(), i + 1, radius, i * self.parking_space_width, (parking_space_num - i - 1) * self.parking_space_width) and no_cross for i in range(parking_space_num, 2 * parking_space_num): index = i + 1 i -= parking_space_num no_cross = self._add_one_parking_space( self.pre_block_socket, copy.copy(self.get_socket_list()[0]), index, radius, i * self.parking_space_width, (parking_space_num - i - 1) * self.parking_space_width) and no_cross return no_cross def _add_one_parking_space(self, in_socket: PGBlockSocket, out_socket: PGBlockSocket, part_idx: int, radius, dist_to_in, dist_to_out) -> bool: no_cross = True # lane into parking space and parking space, 1 if in_socket.is_same_socket( self.pre_block_socket) or in_socket.is_same_socket( self.pre_block_socket.get_socket_in_reverse()): net = self._global_network else: net = self.block_network in_lane = in_socket.positive_road.get_lanes(net)[0] start_node = in_socket.positive_road.end_node if dist_to_in > 1e-3: # a straight part will be added in_lane = ExtendStraightLane(in_lane, dist_to_in, [LineType.NONE, LineType.NONE]) in_road = Road(in_socket.positive_road.end_node, self.road_node(part_idx, 0)) CreateRoadFrom( in_lane, self.positive_lane_num, in_road, self.block_network, self._global_network, center_line_type=LineType.NONE, inner_lane_line_type=LineType.NONE, side_lane_line_type=LineType.NONE, ignore_intersection_checking=self.ignore_intersection_checking) start_node = self.road_node(part_idx, 0) bend, straight = create_bend_straight(in_lane, self.parking_space_length, radius, self.ANGLE, True, self.parking_space_width) bend_road = Road(start_node, self.road_node(part_idx, 1)) bend_no_cross = CreateRoadFrom( bend, self.positive_lane_num, bend_road, self.block_network, self._global_network, center_line_type=LineType.NONE, inner_lane_line_type=LineType.NONE, side_lane_line_type=LineType.SIDE if dist_to_in < 1e-3 else LineType.NONE, ignore_intersection_checking=self.ignore_intersection_checking) if dist_to_in < 1e-3: no_cross = no_cross and bend_no_cross straight_road = Road(self.road_node(part_idx, 1), self.road_node(part_idx, 2)) self.dest_roads.append(straight_road) no_cross = no_cross and CreateRoadFrom( straight, self.positive_lane_num, straight_road, self.block_network, self._global_network, center_line_type=LineType.CONTINUOUS, inner_lane_line_type=LineType.NONE, side_lane_line_type=LineType.SIDE if dist_to_in < 1e-3 else LineType.NONE, center_line_color=LineColor.GREY, ignore_intersection_checking=self.ignore_intersection_checking) # lane into parking space and parking space, 2 neg_road: Road = out_socket.negative_road if out_socket.is_same_socket( self.pre_block_socket) or out_socket.is_same_socket( self.pre_block_socket.get_socket_in_reverse()): net = self._global_network else: net = self.block_network neg_lane = \ neg_road.get_lanes(net)[0] start_node = neg_road.end_node if dist_to_out > 1e-3: # a straight part will be added neg_lane = ExtendStraightLane(neg_lane, dist_to_out, [LineType.NONE, LineType.NONE]) neg_road = Road(neg_road.end_node, self.road_node(part_idx, 3)) CreateRoadFrom( neg_lane, self.positive_lane_num, neg_road, self.block_network, self._global_network, center_line_type=LineType.NONE, inner_lane_line_type=LineType.NONE, side_lane_line_type=LineType.NONE, ignore_intersection_checking=self.ignore_intersection_checking) start_node = self.road_node(part_idx, 3) bend, straight = create_bend_straight(neg_lane, self.lane_width, radius, self.ANGLE, False, self.parking_space_width) bend_road = Road(start_node, self.road_node(part_idx, 4)) CreateRoadFrom( bend, self.positive_lane_num, bend_road, self.block_network, self._global_network, center_line_type=LineType.NONE, inner_lane_line_type=LineType.NONE, side_lane_line_type=LineType.NONE, ignore_intersection_checking=self.ignore_intersection_checking) straight_road = Road(self.road_node(part_idx, 4), self.road_node(part_idx, 1)) CreateRoadFrom( straight, self.positive_lane_num, straight_road, self.block_network, self._global_network, center_line_type=LineType.NONE, inner_lane_line_type=LineType.NONE, side_lane_line_type=LineType.NONE, ignore_intersection_checking=self.ignore_intersection_checking) # give it a new road name to make it be a two way road (1,2) = (5,6) = parking space ! parking_road = Road(self.road_node(part_idx, 5), self.road_node(part_idx, 6)) self.spawn_roads.append(parking_road) CreateTwoWayRoad( Road(self.road_node(part_idx, 1), self.road_node(part_idx, 2)), self.block_network, self._global_network, parking_road, center_line_type=LineType.NONE, inner_lane_line_type=LineType.NONE, side_lane_line_type=LineType.SIDE if dist_to_out < 1e-3 else LineType.NONE, ignore_intersection_checking=self.ignore_intersection_checking) # out part parking_lane = parking_road.get_lanes(self.block_network)[0] # out part 1 bend, straight = create_bend_straight( parking_lane, 0.1 if dist_to_out < 1e-3 else dist_to_out, radius, self.ANGLE, True, parking_lane.width) out_bend_road = Road( self.road_node(part_idx, 6), self.road_node(part_idx, 7) if dist_to_out > 1e-3 else out_socket.positive_road.start_node) bend_success = CreateRoadFrom( bend, self.positive_lane_num, out_bend_road, self.block_network, self._global_network, center_line_type=LineType.NONE, inner_lane_line_type=LineType.NONE, side_lane_line_type=LineType.SIDE if dist_to_out < 1e-3 else LineType.NONE, ignore_intersection_checking=self.ignore_intersection_checking) if dist_to_out < 1e-3: no_cross = no_cross and bend_success if dist_to_out > 1e-3: out_straight_road = Road(self.road_node(part_idx, 7), out_socket.positive_road.start_node) no_cross = no_cross and CreateRoadFrom( straight, self.positive_lane_num, out_straight_road, self.block_network, self._global_network, center_line_type=LineType.NONE, inner_lane_line_type=LineType.NONE, side_lane_line_type=LineType.NONE, ignore_intersection_checking=self.ignore_intersection_checking) # out part 2 extend_lane = ExtendStraightLane(parking_lane, self.lane_width, [LineType.NONE, LineType.NONE]) CreateRoadFrom( extend_lane, self.positive_lane_num, Road(self.road_node(part_idx, 6), self.road_node(part_idx, 8)), self.block_network, self._global_network, center_line_type=LineType.NONE, inner_lane_line_type=LineType.NONE, side_lane_line_type=LineType.NONE, ignore_intersection_checking=self.ignore_intersection_checking) bend, straight = create_bend_straight( extend_lane, 0.1 if dist_to_in < 1e-3 else dist_to_in, radius, self.ANGLE, False, parking_lane.width) out_bend_road = Road( self.road_node(part_idx, 8), self.road_node(part_idx, 9) if dist_to_in > 1e-3 else in_socket.negative_road.start_node) CreateRoadFrom( bend, self.positive_lane_num, out_bend_road, self.block_network, self._global_network, center_line_type=LineType.NONE, inner_lane_line_type=LineType.NONE, side_lane_line_type=LineType.NONE, ignore_intersection_checking=self.ignore_intersection_checking) if dist_to_in > 1e-3: out_straight_road = Road(self.road_node(part_idx, 9), in_socket.negative_road.start_node) CreateRoadFrom( straight, self.positive_lane_num, out_straight_road, self.block_network, self._global_network, center_line_type=LineType.NONE, inner_lane_line_type=LineType.NONE, side_lane_line_type=LineType.NONE, ignore_intersection_checking=self.ignore_intersection_checking) return no_cross @staticmethod def in_direction_parking_space(road: Road): """ Give a parking space in out-direction, return in direction road """ start_node = copy.deepcopy(road.start_node) end_node = copy.deepcopy(road.end_node) assert start_node[-2] == "5" and end_node[ -2] == "6", "It is not out-direction of this parking space, start_node:{}, end_node:{}".format( start_node, end_node) start_node = start_node[:-2] + "1" + PGBlock.DASH end_node = end_node[:-2] + "2" + PGBlock.DASH return Road(start_node, end_node) @staticmethod def out_direction_parking_space(road: Road): """ Give a parking space in in-direction, return out-direction road """ start_node = copy.deepcopy(road.start_node) end_node = copy.deepcopy(road.end_node) assert start_node[-2] == "1" and end_node[ -2] == "2", "It is not in-direction of this parking space, start_node:{}, end_node:{}".format( start_node, end_node) start_node = start_node[:-2] + "5" + PGBlock.DASH end_node = end_node[:-2] + "6" + PGBlock.DASH return Road(start_node, end_node) @staticmethod def is_out_direction_parking_space(road: Road): start_node = road.start_node end_node = road.end_node assert (start_node[-2] == "1" and end_node[-2] == "2") or ( start_node[-2] == "5" and end_node[-2] == "6"), "{} to {} is not parking space".format( start_node, end_node) if start_node[-2] == "5" and end_node[-2] == "6": return True else: return False @staticmethod def is_in_direction_parking_space(road: Road): start_node = road.start_node end_node = road.end_node assert (start_node[-2] == "1" and end_node[-2] == "2") or ( start_node[-2] == "5" and end_node[-2] == "6"), "{} to {} is not parking space".format( start_node, end_node) if start_node[-2] == "1" and end_node[-2] == "2": return True else: return False
class BaseRunnable(Configurable, Nameable, Randomizable): """ Abstract class, all sub class must implement all methods to participate in the program running loop. The BaseRunnable instance can be everything which don't interact with game engine. If you need an element have visualization property or physics property, using BaseObject() instead """ PARAMETER_SPACE = ParameterSpace({}) def __init__(self, name=None, random_seed=None, config=None): Nameable.__init__(self, name) Randomizable.__init__(self, random_seed) Configurable.__init__( self, {k: None for k in self.PARAMETER_SPACE.parameters}) # Parameter check assert isinstance( self.PARAMETER_SPACE, ParameterSpace ), "Using PGSpace to define parameter spaces of " + self.class_name self.sample_parameters() # use external config update to overwrite sampled parameters, except None self.update_config(copy.copy(config), allow_add_new_key=True) def get_state(self) -> Dict: """ Store current state, for example if this runnable instance is an object in the 3D-world state can be heading, position, etc. This function can be used to to store the movement and change history trajectory. :return: state dict """ raise NotImplementedError def set_state(self, state: Dict): """ Set state for this runnable instance, restore the instance to a certain state, For example, if this runnable instance is a policy, it can restore the policy to a certain state to make sure it do the same decision as before :param state: dict """ raise NotImplementedError def before_step(self, *args, **kwargs): """ Do Information fusion and then analyze and wait for decision """ pass def set_action(self, *args, **kwargs): """ Set action for this object, and the action will last for the minimal simulation interval """ raise NotImplementedError def step(self, *args, **kwargs): """ Call this function to implement the decision set by set_action() for a period of time. This function is usually useless, since the result of action, mostly force, is calculated bu game engine via force calculation respect to time. However some runnable instances who don't belong to the physics world and their actions are not force need to implement this function to get the action accumulated result respect to time. """ pass def after_step(self, *args, **kwargs): """ After advancing all objects for a time period, their state should be updated for statistic or other purpose """ def reset(self, random_seed, *args, **kwargs): """ Call this function to re-init objects. Since some __init__ process of creating objects is redundant, reset can help us reuse this object by resetting some necessary attributes """ self.__init__(random_seed=random_seed, *args, **kwargs) def sample_parameters(self): """ Fix a value of the random parameters in PARAMETER_SPACE """ random_seed = self.np_random.randint(low=0, high=int(1e6)) self.PARAMETER_SPACE.seed(random_seed) ret = self.PARAMETER_SPACE.sample() self.update_config(ret) def destroy(self): Configurable.destroy(self)
class TollGate(PGBlock): """ Toll, like Straight, but has speed limit """ SOCKET_NUM = 1 PARAMETER_SPACE = ParameterSpace(BlockParameterSpace.BOTTLENECK_PARAMETER) ID = "$" SPEED_LIMIT = 3 # m/s ~= 5 miles per hour https://bestpass.com/feed/61-speeding-through-tolls def _try_plug_into_previous_block(self) -> bool: self.set_part_idx(0) # only one part in simple block like straight, and curve para = self.get_config() length = para[Parameter.length] self.BUILDING_LENGTH = length basic_lane = self.positive_basic_lane new_lane = ExtendStraightLane(basic_lane, length, [LineType.CONTINUOUS, LineType.SIDE]) start = self.pre_block_socket.positive_road.end_node end = self.add_road_node() socket = Road(start, end) _socket = -socket # create positive road no_cross = CreateRoadFrom( new_lane, self.positive_lane_num, socket, self.block_network, self._global_network, center_line_color=LineColor.YELLOW, center_line_type=LineType.CONTINUOUS, inner_lane_line_type=LineType.CONTINUOUS, side_lane_line_type=LineType.SIDE, ignore_intersection_checking=self.ignore_intersection_checking ) # create negative road no_cross = CreateAdverseRoad( socket, self.block_network, self._global_network, center_line_color=LineColor.YELLOW, center_line_type=LineType.CONTINUOUS, inner_lane_line_type=LineType.CONTINUOUS, side_lane_line_type=LineType.SIDE, ignore_intersection_checking=self.ignore_intersection_checking ) and no_cross self.add_sockets(PGBlockSocket(socket, _socket)) self._add_building_and_speed_limit(socket) self._add_building_and_speed_limit(_socket) return no_cross def _add_building_and_speed_limit(self, road): # add house lanes = road.get_lanes(self.block_network) for idx, lane in enumerate(lanes): lane.set_speed_limit(self.SPEED_LIMIT) if idx % 2 == 1: # add toll position = lane.position(lane.length / 2, 0) building = get_engine().spawn_object( TollGateBuilding, lane=lane, position=position, heading=lane.heading_at(0) ) self.dynamic_nodes.append(building.body) self._block_objects.append(building)
class FirstPGBlock(PGBlock): """ A special Set, only used to create the first block. One scene has only one first block!!! """ NODE_1 = ">" NODE_2 = ">>" NODE_3 = ">>>" PARAMETER_SPACE = ParameterSpace({}) ID = "I" SOCKET_NUM = 1 ENTRANCE_LENGTH = 10 def __init__(self, global_network: RoadNetwork, lane_width: float, lane_num: int, render_root_np: NodePath, physics_world: PhysicsWorld, length: float = 50, ignore_intersection_checking=False): place_holder = PGBlockSocket(Road(Decoration.start, Decoration.end), Road(Decoration.start, Decoration.end)) super(FirstPGBlock, self).__init__( 0, place_holder, global_network, random_seed=0, ignore_intersection_checking=ignore_intersection_checking) assert length > self.ENTRANCE_LENGTH, (length, self.ENTRANCE_LENGTH) self._block_objects = [] basic_lane = StraightLane( [0, lane_width * (lane_num - 1)], [self.ENTRANCE_LENGTH, lane_width * (lane_num - 1)], line_types=(LineType.BROKEN, LineType.SIDE), width=lane_width) ego_v_spawn_road = Road(self.NODE_1, self.NODE_2) CreateRoadFrom( basic_lane, lane_num, ego_v_spawn_road, self.block_network, self._global_network, ignore_intersection_checking=self.ignore_intersection_checking) CreateAdverseRoad( ego_v_spawn_road, self.block_network, self._global_network, ignore_intersection_checking=self.ignore_intersection_checking) next_lane = ExtendStraightLane(basic_lane, length - self.ENTRANCE_LENGTH, [LineType.BROKEN, LineType.SIDE]) other_v_spawn_road = Road(self.NODE_2, self.NODE_3) CreateRoadFrom( next_lane, lane_num, other_v_spawn_road, self.block_network, self._global_network, ignore_intersection_checking=self.ignore_intersection_checking) CreateAdverseRoad( other_v_spawn_road, self.block_network, self._global_network, ignore_intersection_checking=self.ignore_intersection_checking) self._create_in_world() # global_network += self.block_network global_network.add(self.block_network) socket = self.create_socket_from_positive_road(other_v_spawn_road) socket.set_index(self.name, 0) self.add_sockets(socket) self.attach_to_world(render_root_np, physics_world) self._respawn_roads = [other_v_spawn_road]
class Roundabout(PGBlock): """ roundabout class, the example is the same as Intersection """ ID = "O" PARAMETER_SPACE = ParameterSpace(BlockParameterSpace.ROUNDABOUT) SOCKET_NUM = 3 RADIUS_IN = 20 ANGLE = 60 EXIT_PART_LENGTH = 30 def __init__(self, *args, **kwargs): super(Roundabout, self).__init__(*args, **kwargs) self.intermediate_spawn_places = [] def _try_plug_into_previous_block(self) -> bool: self.intermediate_spawn_places = [] para = self.get_config(copy=False) no_cross = True attach_road = self.pre_block_socket.positive_road for i in range(4): exit_road, success = self._create_circular_part( attach_road, i, para[Parameter.radius_exit], para[Parameter.radius_inner], para[Parameter.angle]) no_cross = no_cross and success if i < 3: no_cross = CreateAdverseRoad( exit_road, self.block_network, self._global_network, ignore_intersection_checking=self. ignore_intersection_checking) and no_cross attach_road = -exit_road self.add_respawn_roads( [socket.negative_road for socket in self.get_socket_list()]) return no_cross def _create_circular_part(self, road: Road, part_idx: int, radius_exit: float, radius_inner: float, angle: float) -> (str, str, StraightLane, bool): """ Create a part of roundabout according to a straight road """ none_cross = True self.set_part_idx(part_idx) radius_big = (self.positive_lane_num * 2 - 1) * self.lane_width + radius_inner # circular part 0 segment_start_node = road.end_node segment_end_node = self.add_road_node() segment_road = Road(segment_start_node, segment_end_node) lanes = road.get_lanes( self._global_network) if part_idx == 0 else road.get_lanes( self.block_network) right_lane = lanes[-1] bend, straight = create_bend_straight(right_lane, 10, radius_exit, np.deg2rad(angle), True, self.lane_width, (LineType.BROKEN, LineType.SIDE)) ignore_last_2_part_start = self.road_node((part_idx + 3) % 4, 0) ignore_last_2_part_end = self.road_node((part_idx + 3) % 4, 0) none_cross = CreateRoadFrom( bend, self.positive_lane_num, segment_road, self.block_network, self._global_network, ignore_start=ignore_last_2_part_start, ignore_end=ignore_last_2_part_end, ignore_intersection_checking=self.ignore_intersection_checking ) and none_cross # set circular part 0 visualization for k, lane in enumerate(segment_road.get_lanes(self.block_network)): if k == self.positive_lane_num - 1: lane.line_types = [LineType.NONE, LineType.SIDE] else: lane.line_types = [LineType.NONE, LineType.NONE] # circular part 1 tool_lane_start = straight.position(-5, 0) tool_lane_end = straight.position(0, 0) tool_lane = StraightLane(tool_lane_start, tool_lane_end) bend, straight_to_next_iter_part = create_bend_straight( tool_lane, 10, radius_big, np.deg2rad(2 * angle - 90), False, self.lane_width, (LineType.BROKEN, LineType.SIDE)) segment_start_node = segment_end_node segment_end_node = self.add_road_node() segment_road = Road(segment_start_node, segment_end_node) none_cross = CreateRoadFrom( bend, self.positive_lane_num, segment_road, self.block_network, self._global_network, ignore_intersection_checking=self.ignore_intersection_checking ) and none_cross self.intermediate_spawn_places.append( segment_road.get_lanes(self.block_network)) # circular part 2 and exit straight part length = self.EXIT_PART_LENGTH tool_lane_start = straight_to_next_iter_part.position(-5, 0) tool_lane_end = straight_to_next_iter_part.position(0, 0) tool_lane = StraightLane(tool_lane_start, tool_lane_end) bend, straight = create_bend_straight(tool_lane, length, radius_exit, np.deg2rad(angle), True, self.lane_width, (LineType.BROKEN, LineType.SIDE)) segment_start_node = segment_end_node segment_end_node = self.add_road_node( ) if part_idx < 3 else self.pre_block_socket.negative_road.start_node segment_road = Road(segment_start_node, segment_end_node) none_cross = CreateRoadFrom( bend, self.positive_lane_num, segment_road, self.block_network, self._global_network, ignore_intersection_checking=self.ignore_intersection_checking ) and none_cross # set circular part 2 (curve) visualization for k, lane in enumerate(segment_road.get_lanes(self.block_network)): if k == self.positive_lane_num - 1: lane.line_types = [LineType.NONE, LineType.SIDE] else: lane.line_types = [LineType.NONE, LineType.NONE] exit_start = segment_end_node exit_end = self.add_road_node() segment_road = Road(exit_start, exit_end) if part_idx < 3: none_cross = CreateRoadFrom( straight, self.positive_lane_num, segment_road, self.block_network, self._global_network, ignore_intersection_checking=self.ignore_intersection_checking ) and none_cross self.add_sockets( self.create_socket_from_positive_road(segment_road)) # add circular part 3 at last segment_start = self.road_node(part_idx, 1) segment_end = self.road_node((part_idx + 1) % 4, 0) segment_road = Road(segment_start, segment_end) tool_lane_start = straight_to_next_iter_part.position(-6, 0) tool_lane_end = straight_to_next_iter_part.position(0, 0) tool_lane = StraightLane(tool_lane_start, tool_lane_end) beneath = (self.positive_lane_num * 2 - 1) * self.lane_width / 2 + radius_exit cos = math.cos(np.deg2rad(angle)) radius_this_seg = beneath / cos - radius_exit bend, _ = create_bend_straight(tool_lane, 5, radius_this_seg, np.deg2rad(180 - 2 * angle), False, self.lane_width, (LineType.BROKEN, LineType.SIDE)) CreateRoadFrom( bend, self.positive_lane_num, segment_road, self.block_network, self._global_network, ignore_intersection_checking=self.ignore_intersection_checking) # set circular part 2 visualization for k, lane in enumerate(segment_road.get_lanes(self.block_network)): if k == 0: if self.positive_lane_num > 1: lane.line_types = [LineType.CONTINUOUS, LineType.BROKEN] else: lane.line_types = [LineType.CONTINUOUS, LineType.NONE] else: lane.line_types = [LineType.BROKEN, LineType.BROKEN] return Road(exit_start, exit_end), none_cross def get_socket(self, index: int) -> PGBlockSocket: socket = super(Roundabout, self).get_socket(index) if socket.negative_road in self.get_respawn_roads(): self._respawn_roads.remove(socket.negative_road) return socket def get_intermediate_spawn_lanes(self): """Filter out half of the vehicles.""" return self.get_respawn_lanes() + self.intermediate_spawn_places
class InterSection(PGBlock): """ up(Goal:1) || || || || || _________________||_________________ left(Goal:2) -----------------||---------------- right(Goal:0) __ || | || spawn_road <--| || | || |__ || down It's an Intersection with two lanes on same direction, 4 lanes on both roads """ ID = "X" EXTRA_PART = "extra" PARAMETER_SPACE = ParameterSpace(BlockParameterSpace.INTERSECTION) SOCKET_NUM = 3 ANGLE = 90 # may support other angle in the future EXIT_PART_LENGTH = 30 enable_u_turn = False # LEFT_TURN_NUM = 1 now it is useless def _try_plug_into_previous_block(self) -> bool: para = self.get_config() decrease_increase = -1 if para[Parameter.decrease_increase] == 0 else 1 if self.positive_lane_num <= 1: decrease_increase = 1 elif self.positive_lane_num >= 4: decrease_increase = -1 self.lane_num_intersect = self.positive_lane_num + decrease_increase * para[ Parameter.change_lane_num] no_cross = True attach_road = self.pre_block_socket.positive_road _attach_road = self.pre_block_socket.negative_road attach_lanes = attach_road.get_lanes(self._global_network) # right straight left node name, rotate it to fit different part intersect_nodes = deque([ self.road_node(0, 0), self.road_node(1, 0), self.road_node(2, 0), _attach_road.start_node ]) for i in range(4): right_lane, success = self._create_part(attach_lanes, attach_road, para[Parameter.radius], intersect_nodes, i) no_cross = no_cross and success if i != 3: lane_num = self.positive_lane_num if i == 1 else self.lane_num_intersect exit_road = Road(self.road_node(i, 0), self.road_node(i, 1)) no_cross = CreateRoadFrom( right_lane, lane_num, exit_road, self.block_network, self._global_network, ignore_intersection_checking=self. ignore_intersection_checking) and no_cross no_cross = CreateAdverseRoad( exit_road, self.block_network, self._global_network, ignore_intersection_checking=self. ignore_intersection_checking) and no_cross socket = PGBlockSocket(exit_road, -exit_road) self.add_respawn_roads(socket.negative_road) self.add_sockets(socket) attach_road = -exit_road attach_lanes = attach_road.get_lanes(self.block_network) return no_cross def _create_part(self, attach_lanes, attach_road: Road, radius: float, intersect_nodes: deque, part_idx) -> (StraightLane, bool): lane_num = self.lane_num_intersect if part_idx == 0 or part_idx == 2 else self.positive_lane_num non_cross = True attach_left_lane = attach_lanes[0] # first left part assert isinstance( attach_left_lane, StraightLane ), "Can't create a intersection following a circular lane" self._create_left_turn(radius, lane_num, attach_left_lane, attach_road, intersect_nodes, part_idx) # u-turn if self.enable_u_turn: adverse_road = -attach_road self._create_u_turn(attach_road, part_idx) # go forward part lanes_on_road = copy.deepcopy(attach_lanes) straight_lane_len = 2 * radius + (2 * lane_num - 1) * lanes_on_road[0].width_at(0) for l in lanes_on_road: next_lane = ExtendStraightLane(l, straight_lane_len, (LineType.NONE, LineType.NONE)) self.block_network.add_lane(attach_road.end_node, intersect_nodes[1], next_lane) # right part length = self.EXIT_PART_LENGTH right_turn_lane = lanes_on_road[-1] assert isinstance( right_turn_lane, StraightLane ), "Can't create a intersection following a circular lane" right_bend, right_straight = create_bend_straight( right_turn_lane, length, radius, np.deg2rad(self.ANGLE), True, right_turn_lane.width_at(0), (LineType.NONE, LineType.SIDE)) non_cross = (not check_lane_on_road( self._global_network, right_bend, 1, ignore_intersection_checking=self.ignore_intersection_checking) ) and non_cross CreateRoadFrom( right_bend, min(self.positive_lane_num, self.lane_num_intersect), Road(attach_road.end_node, intersect_nodes[0]), self.block_network, self._global_network, toward_smaller_lane_index=True, side_lane_line_type=LineType.SIDE, inner_lane_line_type=LineType.NONE, center_line_type=LineType.NONE, ignore_intersection_checking=self.ignore_intersection_checking) intersect_nodes.rotate(-1) right_straight.line_types = [LineType.BROKEN, LineType.SIDE] return right_straight, non_cross def get_socket(self, index: int) -> PGBlockSocket: socket = super(InterSection, self).get_socket(index) if socket.negative_road in self.get_respawn_roads(): self._respawn_roads.remove(socket.negative_road) return socket def _create_left_turn(self, radius, lane_num, attach_left_lane, attach_road, intersect_nodes, part_idx): left_turn_radius = radius + lane_num * attach_left_lane.width_at(0) diff = self.lane_num_intersect - self.positive_lane_num # increase lane num if ((part_idx == 1 or part_idx == 3) and diff > 0) or ( (part_idx == 0 or part_idx == 2) and diff < 0): diff = abs(diff) left_bend, extra_part = create_bend_straight( attach_left_lane, self.lane_width * diff, left_turn_radius, np.deg2rad(self.ANGLE), False, attach_left_lane.width_at(0), (LineType.NONE, LineType.NONE)) left_road_start = intersect_nodes[2] pre_left_road_start = left_road_start + self.EXTRA_PART CreateRoadFrom( left_bend, min(self.positive_lane_num, self.lane_num_intersect), Road(attach_road.end_node, pre_left_road_start), self.block_network, self._global_network, toward_smaller_lane_index=False, center_line_type=LineType.NONE, side_lane_line_type=LineType.NONE, inner_lane_line_type=LineType.NONE, ignore_intersection_checking=self.ignore_intersection_checking) CreateRoadFrom( extra_part, min(self.positive_lane_num, self.lane_num_intersect), Road(pre_left_road_start, left_road_start), self.block_network, self._global_network, toward_smaller_lane_index=False, center_line_type=LineType.NONE, side_lane_line_type=LineType.NONE, inner_lane_line_type=LineType.NONE, ignore_intersection_checking=self.ignore_intersection_checking) else: left_bend, _ = create_bend_straight(attach_left_lane, self.EXIT_PART_LENGTH, left_turn_radius, np.deg2rad(self.ANGLE), False, attach_left_lane.width_at(0), (LineType.NONE, LineType.NONE)) left_road_start = intersect_nodes[2] CreateRoadFrom( left_bend, min(self.positive_lane_num, self.lane_num_intersect), Road(attach_road.end_node, left_road_start), self.block_network, self._global_network, toward_smaller_lane_index=False, center_line_type=LineType.NONE, side_lane_line_type=LineType.NONE, inner_lane_line_type=LineType.NONE, ignore_intersection_checking=self.ignore_intersection_checking) def _create_u_turn(self, attach_road, part_idx): # set to CONTINUOUS to debug line_type = LineType.NONE lanes = attach_road.get_lanes( self.block_network) if part_idx != 0 else self.positive_lanes attach_left_lane = lanes[0] lane_num = len(lanes) left_turn_radius = self.lane_width / 2 left_bend, _ = create_bend_straight(attach_left_lane, 0.1, left_turn_radius, np.deg2rad(180), False, attach_left_lane.width_at(0), (LineType.NONE, LineType.NONE)) left_road_start = (-attach_road).start_node CreateRoadFrom( left_bend, lane_num, Road(attach_road.end_node, left_road_start), self.block_network, self._global_network, toward_smaller_lane_index=False, center_line_type=line_type, side_lane_line_type=line_type, inner_lane_line_type=line_type, ignore_intersection_checking=self.ignore_intersection_checking) def add_u_turn(self, enable_u_turn: bool): self.enable_u_turn = enable_u_turn def get_intermediate_spawn_lanes(self): """Override this function for intersection so that we won't spawn vehicles in the center of intersection.""" respawn_lanes = self.get_respawn_lanes() return respawn_lanes
class BaseVehicle(BaseObject, BaseVehicleState): """ Vehicle chassis and its wheels index 0 1 II-----II II-----II | | <---chassis/wheelbase | II-----II 2 3 """ COLLISION_MASK = CollisionGroup.Vehicle PARAMETER_SPACE = ParameterSpace(VehicleParameterSpace.BASE_VEHICLE) MAX_LENGTH = 10 MAX_WIDTH = 2.5 MAX_STEERING = 60 LENGTH = None WIDTH = None HEIGHT = None TIRE_RADIUS = None LATERAL_TIRE_TO_CENTER = None FRONT_WHEELBASE = None REAR_WHEELBASE = None MASS = None CHASSIS_TO_WHEEL_AXIS = 0.2 SUSPENSION_LENGTH = 40 SUSPENSION_STIFFNESS = 30 # for random color choosing MATERIAL_COLOR_COEFF = 10 # to resist other factors, since other setting may make color dark MATERIAL_METAL_COEFF = 1 # 0-1 MATERIAL_ROUGHNESS = 0.8 # smaller to make it more smooth, and reflect more light MATERIAL_SHININESS = 1 # 0-128 smaller to make it more smooth, and reflect more light MATERIAL_SPECULAR_COLOR = (3, 3, 3, 3) # control STEERING_INCREMENT = 0.05 # save memory, load model once model_collection = {} path = None def __init__( self, vehicle_config: Union[dict, Config] = None, name: str = None, random_seed=None, ): """ This Vehicle Config is different from self.get_config(), and it is used to define which modules to use, and module parameters. And self.physics_config defines the physics feature of vehicles, such as length/width :param vehicle_config: mostly, vehicle module config :param random_seed: int """ # check assert vehicle_config is not None, "Please specify the vehicle config." assert engine_initialized( ), "Please make sure game engine is successfully initialized!" # NOTE: it is the game engine, not vehicle drivetrain self.engine = get_engine() BaseObject.__init__(self, name, random_seed, self.engine.global_config["vehicle_config"]) BaseVehicleState.__init__(self) self.update_config(vehicle_config) am_i_the_special_one = self.config["am_i_the_special_one"] # build vehicle physics model vehicle_chassis = self._create_vehicle_chassis() self.add_body(vehicle_chassis.getChassis()) self.system = vehicle_chassis self.chassis = self.origin self.wheels = self._create_wheel() # powertrain config self.increment_steering = self.config["increment_steering"] self.enable_reverse = self.config["enable_reverse"] self.max_speed = self.config["max_speed"] self.max_steering = self.config["max_steering"] # visualization color = sns.color_palette("colorblind") idx = get_np_random().randint(len(color)) rand_c = color[idx] if am_i_the_special_one: rand_c = color[2] # A pretty green self.top_down_color = (rand_c[0] * 255, rand_c[1] * 255, rand_c[2] * 255) self.panda_color = rand_c self._add_visualization() # modules, get observation by using these modules self.lane: Optional[AbstractLane] = None self.lane_index = None self.navigation: Optional[Navigation] = None self.lidar: Optional[Lidar] = None # detect surrounding vehicles self.side_detector: Optional[SideDetector] = None # detect road side self.lane_line_detector: Optional[ LaneLineDetector] = None # detect nearest lane lines self.image_sensors = {} # state info self.throttle_brake = 0.0 self.steering = 0 self.last_current_action = deque([(0.0, 0.0), (0.0, 0.0)], maxlen=2) self.last_position = (0, 0) self.last_heading_dir = self.heading self.dist_to_left_side = None self.dist_to_right_side = None # step info self.out_of_route = None self.on_lane = None self.spawn_place = (0, 0) self._init_step_info() # others self._add_modules_for_vehicle() self.takeover = False self.expert_takeover = False self.energy_consumption = 0 self.action_space = self.get_action_space_before_init( extra_action_dim=self.config["extra_action_dim"]) self.break_down = False # overtake_stat self.front_vehicles = set() self.back_vehicles = set() if self.engine.current_map is not None: self.reset() def _add_modules_for_vehicle(self, ): config = self.config # add routing module self.add_navigation() # default added # add distance detector/lidar self.side_detector = SideDetector( config["side_detector"]["num_lasers"], config["side_detector"]["distance"], self.engine.global_config["vehicle_config"]["show_side_detector"]) self.lane_line_detector = LaneLineDetector( config["lane_line_detector"]["num_lasers"], config["lane_line_detector"]["distance"], self.engine.global_config["vehicle_config"] ["show_lane_line_detector"]) self.lidar = Lidar( config["lidar"]["num_lasers"], config["lidar"]["distance"], self.engine.global_config["vehicle_config"]["show_lidar"]) # vision modules self.add_image_sensor("rgb_camera", RGBCamera()) self.add_image_sensor("mini_map", MiniMap()) self.add_image_sensor("depth_camera", DepthCamera()) def _init_step_info(self): # done info will be initialized every frame self.init_state_info() self.out_of_route = False # re-route is required if is false self.on_lane = True # on lane surface or not def _preprocess_action(self, action): if self.config["action_check"]: assert self.action_space.contains( action ), "Input {} is not compatible with action space {}!".format( action, self.action_space) return action, {'raw_action': (action[0], action[1])} def before_step(self, action): """ Save info and make decision before action """ # init step info to store info before each step self._init_step_info() action, step_info = self._preprocess_action(action) self.last_position = self.position self.last_heading_dir = self.heading self.last_current_action.append( action ) # the real step of physics world is implemented in taskMgr.step() if self.increment_steering: self._set_incremental_action(action) else: self._set_action(action) return step_info def after_step(self): if self.navigation is not None: self.lane, self.lane_index, = self.navigation.update_localization( self) self._state_check() self.update_dist_to_left_right() step_energy, episode_energy = self._update_energy_consumption() self.out_of_route = self._out_of_route() step_info = self._update_overtake_stat() step_info.update({ "velocity": float(self.speed), "steering": float(self.steering), "acceleration": float(self.throttle_brake), "step_energy": step_energy, "episode_energy": episode_energy }) return step_info def _out_of_route(self): left, right = self._dist_to_route_left_right() return True if right < 0 or left < 0 else False 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 reset(self, random_seed=None, vehicle_config=None, pos: np.ndarray = None, heading: float = 0.0): """ pos is a 2-d array, and heading is a float (unit degree) if pos is not None, vehicle will be reset to the position else, vehicle will be reset to spawn place """ if random_seed is not None: self.seed(random_seed) self.sample_parameters() if vehicle_config is not None: self.update_config(vehicle_config) map = self.engine.current_map if pos is None: lane = map.road_network.get_lane(self.config["spawn_lane_index"]) pos = lane.position(self.config["spawn_longitude"], self.config["spawn_lateral"]) heading = np.rad2deg( lane.heading_at(self.config["spawn_longitude"])) self.spawn_place = pos heading = -np.deg2rad(heading) - np.pi / 2 self.set_static(False) self.origin.setPos(panda_position(Vec3(*pos, self.HEIGHT / 2 + 1))) self.origin.setQuat( LQuaternionf(math.cos(heading / 2), 0, 0, math.sin(heading / 2))) self.update_map_info(map) self.body.clearForces() self.body.setLinearVelocity(Vec3(0, 0, 0)) self.body.setAngularVelocity(Vec3(0, 0, 0)) self.system.resetSuspension() self._apply_throttle_brake(0.0) # np.testing.assert_almost_equal(self.position, pos, decimal=4) # done info self._init_step_info() # other info self.throttle_brake = 0.0 self.steering = 0 self.last_current_action = deque([(0.0, 0.0), (0.0, 0.0)], maxlen=2) self.last_position = self.spawn_place self.last_heading_dir = self.heading self.update_dist_to_left_right() self.takeover = False self.energy_consumption = 0 # overtake_stat self.front_vehicles = set() self.back_vehicles = set() assert self.navigation """------------------------------------------- act -------------------------------------------------""" def _set_action(self, action): steering = action[0] self.throttle_brake = action[1] self.steering = steering self.system.setSteeringValue(self.steering * self.max_steering, 0) self.system.setSteeringValue(self.steering * self.max_steering, 1) self._apply_throttle_brake(action[1]) def _set_incremental_action(self, action: np.ndarray): self.throttle_brake = action[1] self.steering += action[0] * self.STEERING_INCREMENT self.steering = clip(self.steering, -1, 1) steering = self.steering * self.max_steering self.system.setSteeringValue(steering, 0) self.system.setSteeringValue(steering, 1) self._apply_throttle_brake(action[1]) def _apply_throttle_brake(self, throttle_brake): max_engine_force = self.config["max_engine_force"] max_brake_force = self.config["max_brake_force"] for wheel_index in range(4): if throttle_brake >= 0: self.system.setBrake(2.0, wheel_index) if self.speed > self.max_speed: self.system.applyEngineForce(0.0, wheel_index) else: self.system.applyEngineForce( max_engine_force * throttle_brake, wheel_index) else: if self.enable_reverse: self.system.applyEngineForce( max_engine_force * throttle_brake, wheel_index) self.system.setBrake(0, wheel_index) else: self.system.applyEngineForce(0.0, wheel_index) self.system.setBrake( abs(throttle_brake) * max_brake_force, wheel_index) """---------------------------------------- vehicle info ----------------------------------------------""" def update_dist_to_left_right(self): self.dist_to_left_side, self.dist_to_right_side = self._dist_to_route_left_right( ) def _dist_to_route_left_right(self): current_reference_lane = self.navigation.current_ref_lanes[0] _, lateral_to_reference = current_reference_lane.local_coordinates( self.position) lateral_to_left = lateral_to_reference + self.navigation.get_current_lane_width( ) / 2 lateral_to_right = self.navigation.get_current_lateral_range( self.position, self.engine) - lateral_to_left return lateral_to_left, lateral_to_right @property def position(self): return pgdrive_position(self.origin.getPos()) @property 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) @property def heading(self): real_heading = self.heading_theta # heading = np.array([math.cos(real_heading), math.sin(real_heading)]) heading = Vector((math.cos(real_heading), math.sin(real_heading))) return heading @property def heading_theta(self): """ Get the heading theta of vehicle, unit [rad] :return: heading in rad """ return (pgdrive_heading(self.origin.getH()) - 90) / 180 * math.pi @property def velocity(self) -> np.ndarray: return self.speed * self.velocity_direction @property def velocity_direction(self): direction = self.system.getForwardVector() return np.asarray([direction[0], -direction[1]]) @property def current_road(self): return Road(*self.lane_index[0:-1]) """---------------------------------------- some math tool ----------------------------------------------""" 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 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 lane_distance_to(self, vehicle, lane: AbstractLane = None) -> float: assert self.navigation is not None, "a routing and localization module should be added " \ "to interact with other vehicles" if not vehicle: return np.nan if not lane: lane = self.lane return lane.local_coordinates( vehicle.position)[0] - lane.local_coordinates(self.position)[0] """-------------------------------------- for vehicle making ------------------------------------------""" def _create_vehicle_chassis(self): self.LENGTH = type(self).LENGTH self.WIDTH = type(self).WIDTH self.HEIGHT = type(self).HEIGHT assert self.LENGTH < BaseVehicle.MAX_LENGTH, "Vehicle is too large!" assert self.WIDTH < BaseVehicle.MAX_WIDTH, "Vehicle is too large!" chassis = BaseRigidBodyNode(self.name, BodyName.Vehicle) chassis.setIntoCollideMask(CollisionGroup.Vehicle) chassis_shape = BulletBoxShape( Vec3(self.WIDTH / 2, self.LENGTH / 2, self.HEIGHT / 2)) ts = TransformState.makePos(Vec3(0, 0, self.HEIGHT / 2)) chassis.addShape(chassis_shape, ts) chassis.setMass(self.MASS) chassis.setDeactivationEnabled(False) chassis.notifyCollisions( True ) # advance collision check, do callback in pg_collision_callback physics_world = get_engine().physics_world vehicle_chassis = BulletVehicle(physics_world.dynamic_world, chassis) vehicle_chassis.setCoordinateSystem(ZUp) self.dynamic_nodes.append(vehicle_chassis) return vehicle_chassis def _add_visualization(self): if self.render: [path, scale, x_y_z_offset, H] = self.path[self.np_random.randint(0, len(self.path))] if path not in BaseVehicle.model_collection: car_model = self.loader.loadModel( AssetLoader.file_path("models", path)) BaseVehicle.model_collection[path] = car_model else: car_model = BaseVehicle.model_collection[path] car_model.setScale(scale) car_model.setH(H) car_model.setPos(x_y_z_offset) car_model.setZ(-self.TIRE_RADIUS - self.CHASSIS_TO_WHEEL_AXIS + x_y_z_offset[-1]) car_model.instanceTo(self.origin) if self.config["random_color"]: material = Material() material.setBaseColor( (self.panda_color[0] * self.MATERIAL_COLOR_COEFF, self.panda_color[1] * self.MATERIAL_COLOR_COEFF, self.panda_color[2] * self.MATERIAL_COLOR_COEFF, 0.2)) material.setMetallic(self.MATERIAL_METAL_COEFF) material.setSpecular(self.MATERIAL_SPECULAR_COLOR) material.setRefractiveIndex(1.5) material.setRoughness(self.MATERIAL_ROUGHNESS) material.setShininess(self.MATERIAL_SHININESS) material.setTwoside(False) self.origin.setMaterial(material, True) def _create_wheel(self): f_l = self.FRONT_WHEELBASE r_l = -self.REAR_WHEELBASE lateral = self.LATERAL_TIRE_TO_CENTER axis_height = self.TIRE_RADIUS - self.CHASSIS_TO_WHEEL_AXIS radius = self.TIRE_RADIUS wheels = [] for k, pos in enumerate([ Vec3(lateral, f_l, axis_height), Vec3(-lateral, f_l, axis_height), Vec3(lateral, r_l, axis_height), Vec3(-lateral, r_l, axis_height) ]): wheel = self._add_wheel(pos, radius, True if k < 2 else False, True if k == 0 or k == 2 else False) wheels.append(wheel) return wheels def _add_wheel(self, pos: Vec3, radius: float, front: bool, left): wheel_np = self.origin.attachNewNode("wheel") if self.render: # TODO something wrong with the wheel render model_path = 'models/yugo/yugotireR.egg' if left else 'models/yugo/yugotireL.egg' wheel_model = self.loader.loadModel( AssetLoader.file_path(model_path)) wheel_model.reparentTo(wheel_np) wheel_model.set_scale(1.4, radius / 0.25, radius / 0.25) wheel = self.system.create_wheel() wheel.setNode(wheel_np.node()) wheel.setChassisConnectionPointCs(pos) wheel.setFrontWheel(front) wheel.setWheelDirectionCs(Vec3(0, 0, -1)) wheel.setWheelAxleCs(Vec3(1, 0, 0)) wheel.setWheelRadius(radius) wheel.setMaxSuspensionTravelCm(self.SUSPENSION_LENGTH) wheel.setSuspensionStiffness(self.SUSPENSION_STIFFNESS) wheel.setWheelsDampingRelaxation(4.8) wheel.setWheelsDampingCompression(1.2) wheel.setFrictionSlip(self.config["wheel_friction"]) wheel.setRollInfluence(0.5) return wheel def add_image_sensor(self, name: str, sensor: ImageBuffer): self.image_sensors[name] = sensor def add_navigation(self): self.navigation = Navigation( self.engine, show_navi_mark=self.engine.global_config["vehicle_config"] ["show_navi_mark"], random_navi_mark_color=self.engine.global_config["vehicle_config"] ["random_navi_mark_color"], show_dest_mark=self.engine.global_config["vehicle_config"] ["show_dest_mark"], show_line_to_dest=self.engine.global_config["vehicle_config"] ["show_line_to_dest"]) def update_map_info(self, map): """ Update map info after reset() :param map: new map :return: None """ possible_lanes = ray_localization(self.heading, self.spawn_place, self.engine, return_all_result=True) possible_lane_indexes = [ lane_index for lane, lane_index, dist in possible_lanes ] try: idx = possible_lane_indexes.index(self.config["spawn_lane_index"]) except ValueError: lane, new_l_index = possible_lanes[0][:-1] else: lane, new_l_index = possible_lanes[idx][:-1] dest = self.config["destination_node"] self.navigation.update( map, current_lane_index=new_l_index, final_road_node=dest if dest is not None else None, random_seed=self.engine.global_random_seed) assert lane is not None, "spawn place is not on road!" self.navigation.update_localization(self) self.lane_index = new_l_index self.lane = lane def _state_check(self): """ Check States and filter to update info """ result_1 = self.engine.physics_world.static_world.contactTest( self.chassis.node(), True) result_2 = self.engine.physics_world.dynamic_world.contactTest( self.chassis.node(), True) contacts = set() for contact in result_1.getContacts() + result_2.getContacts(): node0 = contact.getNode0() node1 = contact.getNode1() name = [node0.getName(), node1.getName()] name.remove(BodyName.Vehicle) if name[0] == BodyName.White_continuous_line: self.on_white_continuous_line = True elif name[0] == BodyName.Yellow_continuous_line: self.on_yellow_continuous_line = True elif name[0] == BodyName.Broken_line: self.on_broken_line = True else: # didn't add continue contacts.add(name[0]) # side walk detect res = rect_region_detection(self.engine, self.position, np.rad2deg(self.heading_theta), self.LENGTH, self.WIDTH, CollisionGroup.Sidewalk) if res.hasHit() and res.getNode().getName() == BodyName.Sidewalk: self.crash_sidewalk = True contacts.add(BodyName.Sidewalk) self.contact_results = contacts def destroy(self): super(BaseVehicle, self).destroy() self.navigation.destroy() self.navigation = None if self.side_detector is not None: self.side_detector.destroy() self.side_detector = None if self.lane_line_detector is not None: self.lane_line_detector.destroy() self.lane_line_detector = None if self.lidar is not None: self.lidar.destroy() self.lidar = None if len(self.image_sensors) != 0: for sensor in self.image_sensors.values(): sensor.destroy() self.image_sensors = None self.engine = None def set_position(self, position, height=0.4): """ Should only be called when restore traffic from episode data :param position: 2d array or list :return: None """ self.origin.setPos(panda_position(position, height)) def set_heading(self, heading_theta) -> None: """ Should only be called when restore traffic from episode data :param heading_theta: float in rad :return: None """ self.origin.setH((panda_heading(heading_theta) * 180 / np.pi) - 90) def get_state(self): final_road = self.navigation.final_road return { "heading": self.heading_theta, "position": self.position.tolist(), "done": self.crash_vehicle or self.out_of_route or self.crash_sidewalk or not self.on_lane, "speed": self.speed, "spawn_road": self.config["spawn_lane_index"][:-1], "destination": (final_road.start_node, final_road.end_node) } def set_state(self, state: dict): self.set_heading(state["heading"]) self.set_position(state["position"]) self._replay_done = state["done"] self.set_position(state["position"], height=0.28) def _update_overtake_stat(self): if self.config["overtake_stat"] and self.lidar.available: surrounding_vs = self.lidar.get_surrounding_vehicles() routing = self.navigation ckpt_idx = routing._target_checkpoints_index for surrounding_v in surrounding_vs: if surrounding_v.lane_index[:-1] == ( routing.checkpoints[ckpt_idx[0]], routing.checkpoints[ckpt_idx[1]]): if self.lane.local_coordinates(self.position)[0] - \ self.lane.local_coordinates(surrounding_v.position)[0] < 0: self.front_vehicles.add(surrounding_v) if surrounding_v in self.back_vehicles: self.back_vehicles.remove(surrounding_v) else: self.back_vehicles.add(surrounding_v) return {"overtake_vehicle_num": self.get_overtake_num()} def get_overtake_num(self): return len(self.front_vehicles.intersection(self.back_vehicles)) @classmethod def get_action_space_before_init(cls, extra_action_dim: int = 0, discrete_action=False, steering_dim=5, throttle_dim=5): if not discrete_action: return gym.spaces.Box(-1.0, 1.0, shape=(2 + extra_action_dim, ), dtype=np.float32) else: return gym.spaces.MultiDiscrete([steering_dim, throttle_dim]) def __del__(self): super(BaseVehicle, self).__del__() self.engine = None self.lidar = None self.mini_map = None self.rgb_camera = None self.navigation = None self.wheels = None @property def arrive_destination(self): long, lat = self.navigation.final_lane.local_coordinates(self.position) flag = (self.navigation.final_lane.length - 5 < long < self.navigation.final_lane.length + 5) and ( self.navigation.get_current_lane_width() / 2 >= lat >= (0.5 - self.navigation.get_current_lane_num()) * self.navigation.get_current_lane_width()) return flag def set_static(self, flag): self.body.setStatic(flag) @property def reference_lanes(self): return self.navigation.current_ref_lanes def set_wheel_friction(self, new_friction): raise DeprecationWarning("Bug exists here") for wheel in self.wheels: wheel.setFrictionSlip(new_friction) @property def overspeed(self): return True if self.lane.speed_limit < self.speed else False @property def replay_done(self): return self._replay_done if hasattr(self, "_replay_done") else ( self.crash_building or self.crash_vehicle or # self.on_white_continuous_line or self.on_yellow_continuous_line) @property def current_action(self): return self.last_current_action[-1] @property def last_current(self): return self.last_current_action[0] def detach_from_world(self, physics_world): self.navigation.detach_from_world() if self.lidar is not None: self.lidar.detach_from_world() if self.side_detector is not None: self.side_detector.detach_from_world() if self.lane_line_detector is not None: self.lane_line_detector.detach_from_world() super(BaseVehicle, self).detach_from_world(physics_world) def attach_to_world(self, parent_node_path, physics_world): if self.config["show_navi_mark"]: self.navigation.attach_to_world(self.engine) if self.lidar is not None and self.config["show_lidar"]: self.lidar.attach_to_world(self.engine) if self.side_detector is not None and self.config["show_side_detector"]: self.side_detector.attach_to_world(self.engine) if self.lane_line_detector is not None and self.config[ "show_lane_line_detector"]: self.lane_line_detector.attach_to_world(self.engine) super(BaseVehicle, self).attach_to_world(parent_node_path, physics_world) def set_break_down(self, break_down=True): self.break_down = break_down
class TInterSection(InterSection): """ A structure like X Intersection, code copied from it mostly """ ID = "T" SOCKET_NUM = 2 PARAMETER_SPACE = ParameterSpace(BlockParameterSpace.T_INTERSECTION) def _try_plug_into_previous_block(self) -> bool: no_cross = super(TInterSection, self)._try_plug_into_previous_block() self._exclude_lanes() return no_cross def _change_vis(self, t_type): # not good here, next_part_socket = self.get_socket_list()[(t_type + 1) % 4] # next_part_socket = self._sockets[(t_type + 1) % 4] # FIXME pzh: Help! @LQY What is in this part? next_positive = next_part_socket.positive_road next_negative = next_part_socket.negative_road last_part_socket = self.get_socket_list()[(t_type + 3) % 4] # last_part_socket = self._sockets[(t_type + 3) % 4] # FIXME pzh: Help! @LQY last_positive = last_part_socket.positive_road last_negative = last_part_socket.negative_road if t_type == Goal.LEFT: next_positive = next_part_socket.negative_road next_negative = next_part_socket.positive_road if t_type == Goal.RIGHT: last_positive = last_part_socket.negative_road last_negative = last_part_socket.positive_road for i, road in enumerate([Road(last_negative.end_node, next_positive.start_node), Road(next_negative.end_node, last_positive.start_node)]): lanes = road.get_lanes(self.block_network) outside_type = LineType.SIDE if i == 0 else LineType.NONE for k, lane in enumerate(lanes): line_types = [LineType.BROKEN, LineType.BROKEN ] if k != len(lanes) - 1 else [LineType.BROKEN, outside_type] lane.line_types = line_types if k == 0: lane.line_color = [LineColor.YELLOW, LineColor.GREY] if i == 1: lane.line_types[0] = LineType.NONE def _exclude_lanes(self): para = self.get_config() t_type = para[Parameter.t_intersection_type] self.add_sockets(self.pre_block_socket) start_node = self._sockets[PGBlockSocket.get_real_index(self.name, t_type)].negative_road.end_node end_node = self._sockets[PGBlockSocket.get_real_index(self.name, t_type)].positive_road.start_node for i in range(4): if i == t_type: continue index_i = PGBlockSocket.get_real_index(self.name, i) if i < 3 else self.pre_block_socket_index exit_node = self._sockets[index_i].positive_road.start_node if i != Goal.ADVERSE else self._sockets[ index_i].negative_road.start_node pos_lanes = self.block_network.remove_all_roads(start_node, exit_node) entry_node = self._sockets[index_i].negative_road.end_node if i != Goal.ADVERSE else self._sockets[ index_i].positive_road.end_node neg_lanes = self.block_network.remove_all_roads(entry_node, end_node) # if (i + 2) % 4 == t_type: # # for vis only, solve a bug existing in a corner case, # for lane in neg_lanes: # lane.reset_start_end(lane.start, lane.position(lane.length / 2, 0)) # self.block_network.add_road(Road(Decoration.start, Decoration.end), neg_lanes) # # for lane in pos_lanes: # lane.reset_start_end(lane.position(lane.length / 2, 0), lane.end) # self.block_network.add_road(Road(Decoration.start, Decoration.end), pos_lanes) self._change_vis(t_type) self._sockets.pop(self.pre_block_socket.index) socket = self._sockets.pop(PGBlockSocket.get_real_index(self.name, t_type)) self.block_network.remove_all_roads(socket.positive_road.start_node, socket.positive_road.end_node) self.block_network.remove_all_roads(socket.negative_road.start_node, socket.negative_road.end_node) self._respawn_roads.remove(socket.negative_road) def _add_one_socket(self, socket: PGBlockSocket): assert isinstance(socket, PGBlockSocket), "Socket list only accept BlockSocket Type" # mute warning in T interection # if socket.index is not None and not socket.index.startswith(self.name): # logging.warning( # "The adding socket has index {}, which is not started with this block name {}. This is dangerous! " # "Current block has sockets: {}.".format(socket.index, self.name, self.get_socket_indices()) # ) if socket.index is None: # if this socket is self block socket socket.set_index(self.name, len(self._sockets)) self._sockets[socket.index] = socket def add_u_turn(self, enable_u_turn: bool): raise ValueError("T intersection didn't support u_turn now")