class Straight(Block): """ Straight Road ---------------------------------------- ---------------------------------------- ---------------------------------------- """ ID = "S" SOCKET_NUM = 1 PARAMETER_SPACE = PGSpace(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.STRIPED, 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) # create negative road no_cross = CreateAdverseRoad(socket, self.block_network, self._global_network) and no_cross self.add_sockets(BlockSocket(socket, _socket)) return no_cross
class Ramp(Block): """ InRamp OutRamp start ----------- end ------------------ start ----------- end ------------------ start ----------- end ------------------ start ----------- end ------------------ (start ----------- end)[----------------] start ----------- end [----------------] end -----------------} (start ---------- {end) // \\ { ---------------// \\---------------} """ PARAMETER_SPACE = PGSpace(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 def __init__(self, block_index: int, pre_block_socket: BlockSocket, global_network: RoadNetwork, random_seed): super(Ramp, self).__init__(block_index, pre_block_socket, global_network, random_seed)
class Curve(Block): """ 2 - - - - - - - - - - / 3 - - - - - - - - - - / / / / 0 1 """ ID = "C" SOCKET_NUM = 1 PARAMETER_SPACE = PGSpace(BlockParameterSpace.CURVE) def _try_plug_into_previous_block(self) -> bool: parameters = self.get_config() basic_lane = self.positive_basic_lane lane_num = self.positive_lane_num # part 1 start_node = self._pre_block_socket.positive_road.end_node end_node = self.add_road_node() positive_road = Road(start_node, end_node) length = parameters[Parameter.length] direction = parameters[Parameter.dir] angle = parameters[Parameter.angle] radius = parameters[Parameter.radius] curve, straight = sharpbend(basic_lane, length, radius, np.deg2rad(angle), direction, basic_lane.width, (LineType.STRIPED, LineType.SIDE)) no_cross = CreateRoadFrom(curve, lane_num, positive_road, self.block_network, self._global_network) no_cross = CreateAdverseRoad(positive_road, self.block_network, self._global_network) and no_cross # part 2 start_node = end_node end_node = self.add_road_node() positive_road = Road(start_node, end_node) no_cross = CreateRoadFrom(straight, lane_num, positive_road, self.block_network, self._global_network) and no_cross no_cross = CreateAdverseRoad(positive_road, self.block_network, self._global_network) and no_cross # common properties self.add_sockets(self.create_socket_from_positive_road(positive_road)) return no_cross
class FirstBlock(Block): """ 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 = PGSpace({}) ID = "I" SOCKET_NUM = 1 def __init__(self, global_network: RoadNetwork, lane_width: float, lane_num: int, render_root_np: NodePath, pg_physics_world: PGPhysicsWorld, random_seed): place_holder = BlockSocket(Road(Decoration.start, Decoration.end), Road(Decoration.start, Decoration.end)) super(FirstBlock, self).__init__(0, place_holder, global_network, random_seed) basic_lane = StraightLane([0, lane_width * (lane_num - 1)], [10, lane_width * (lane_num - 1)], line_types=(LineType.STRIPED, LineType.SIDE), width=lane_width) ego_v_born_road = Road(self.NODE_1, self.NODE_2) CreateRoadFrom(basic_lane, lane_num, ego_v_born_road, self.block_network, self._global_network) CreateAdverseRoad(ego_v_born_road, self.block_network, self._global_network) next_lane = ExtendStraightLane(basic_lane, 40, [LineType.STRIPED, LineType.SIDE]) other_v_born_road = Road(self.NODE_2, self.NODE_3) CreateRoadFrom(next_lane, lane_num, other_v_born_road, self.block_network, self._global_network) CreateAdverseRoad(other_v_born_road, self.block_network, self._global_network) self._create_in_world() global_network += self.block_network socket = self.create_socket_from_positive_road(other_v_born_road) socket.index = 0 self.add_sockets(socket) self.attach_to_pg_world(render_root_np, pg_physics_world) self._reborn_roads = [other_v_born_road]
class Fork(Ramp): """ Similar to Ramp """ PARAMETER_SPACE = PGSpace(BlockParameterSpace.FORK_PARAMETER)
class BaseVehicle(DynamicElement): Ego_state_obs_dim = 9 """ Vehicle chassis and its wheels index 0 1 II-----II | | <---chassis | II-----II 2 3 """ PARAMETER_SPACE = PGSpace( VehicleParameterSpace.BASE_VEHICLE ) # it will not sample config from parameter space COLLISION_MASK = CollisionGroup.EgoVehicle STEERING_INCREMENT = 0.05 default_vehicle_config = PGConfig( dict( lidar=(240, 50, 4), # laser num, distance, other vehicle info num mini_map=(84, 84, 250), # buffer length, width rgb_cam=(160, 120), # buffer length, width depth_cam=(84, 84, True), # buffer length, width, view_ground show_navi_mark=False, increment_steering=False, wheel_friction=0.6, )) born_place = (5, 0) LENGTH = None WIDTH = None def __init__(self, pg_world: PGWorld, vehicle_config: dict = None, random_seed: int = 0, config: dict = None): """ This Vehicle Config is different from self.get_config(), and it is used to define which modules to use, and module parameters. """ super(BaseVehicle, self).__init__(random_seed) self.pg_world = pg_world self.node_path = NodePath("vehicle") # config info self.set_config(self.PARAMETER_SPACE.sample()) if config is not None: self.set_config(config) self.vehicle_config = self.get_vehicle_config( vehicle_config ) if vehicle_config is not None else self.default_vehicle_config self.increment_steering = self.vehicle_config["increment_steering"] self.max_speed = self.get_config()[Parameter.speed_max] self.max_steering = self.get_config()[Parameter.steering_max] # create self._add_chassis(pg_world.physics_world) self.wheels = self._create_wheel() # modules self.image_sensors = {} self.lidar = None self.routing_localization = None self.lane = None self.lane_index = None self.vehicle_panel = VehiclePanel(self.pg_world) if ( self.pg_world.mode == RENDER_MODE_ONSCREEN) else None # 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.born_place self.last_heading_dir = self.heading # collision info render self.collision_info_np = self._init_collision_info_render(pg_world) self.collision_banners = {} # to save time self.current_banner = None # done info self.crash = False self.out_of_road = False self.attach_to_pg_world(self.pg_world.pbr_render, self.pg_world.physics_world) @classmethod def get_vehicle_config(cls, new_config): default = copy.deepcopy(cls.default_vehicle_config) default.update(new_config) return default def prepare_step(self, action): """ Save info and make decision before 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_act(action) if self.vehicle_panel is not None: self.vehicle_panel.renew_2d_car_para_visualization(self) def update_state(self, pg_world=None): if self.lidar is not None: self.lidar.perceive(self.position, self.heading_theta, self.pg_world.physics_world) if self.routing_localization is not None: self.lane, self.lane_index = self.routing_localization.update_navigation_localization( self) return self._state_check() def reset(self, map: Map, pos: np.ndarray, heading: float): """ pos is a 2-d array, and heading is a float (unit degree) """ heading = -np.deg2rad(heading) - np.pi / 2 self.chassis_np.setPos(Vec3(*pos, 1)) self.chassis_np.setQuat( LQuaternionf(np.cos(heading / 2), 0, 0, np.sin(heading / 2))) self.update_map_info(map) self.chassis_np.node().clearForces() self.chassis_np.node().setLinearVelocity(Vec3(0, 0, 0)) self.chassis_np.node().setAngularVelocity(Vec3(0, 0, 0)) self.system.resetSuspension() # done info self.crash = False self.out_of_road = False # 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.born_place self.last_heading_dir = self.heading if "depth_cam" in self.image_sensors and self.image_sensors[ "depth_cam"].view_ground: for block in map.blocks: block.node_path.hide(CamMask.DepthCam) """------------------------------------------- act -------------------------------------------------""" def set_act(self, action): para = self.get_config() steering = action[0] self.throttle_brake = action[1] self.steering = steering self.system.setSteeringValue( self.steering * para[Parameter.steering_max], 0) self.system.setSteeringValue( self.steering * para[Parameter.steering_max], 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): para = self.get_config() max_engine_force = para[Parameter.engine_force_max] max_brake_force = para[Parameter.brake_force_max] 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: self.system.applyEngineForce(0.0, wheel_index) self.system.setBrake( abs(throttle_brake) * max_brake_force, wheel_index) """---------------------------------------- vehicle info ----------------------------------------------""" @property def position(self): return pgdrive_position(self.chassis_np.getPos()) @property def speed(self): """ km/h """ speed = self.chassis_np.node().get_linear_velocity().length() * 3.6 return clip(speed, 0.0, 100000.0) @property def heading(self): real_heading = self.heading_theta heading = np.array([np.cos(real_heading), np.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.chassis_np.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.get_forward_vector() return np.asarray([direction[0], -direction[1]]) @property def forward_direction(self): raise ValueError("This function id deprecated.") # print("This function id deprecated.") # direction = self.vehicle.get_forward_vector() # return np.array([direction[0], -direction[1]]) @property def current_road(self): return 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 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 = self.forward_direction.dot(lateral) / (np.linalg.norm(lateral) * np.linalg.norm(self.forward_direction)) 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.routing_localization 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 _add_chassis(self, pg_physics_world: PGPhysicsWorld): para = self.get_config() chassis = BulletRigidBodyNode(BodyName.Ego_vehicle_top) chassis.setIntoCollideMask(BitMask32.bit(self.COLLISION_MASK)) chassis_shape = BulletBoxShape( Vec3(para[Parameter.vehicle_width] / 2, para[Parameter.vehicle_length] / 2, para[Parameter.vehicle_height] / 2)) ts = TransformState.makePos( Vec3(0, 0, para[Parameter.chassis_height] * 2)) chassis.addShape(chassis_shape, ts) heading = np.deg2rad(-para[Parameter.heading] - 90) chassis.setMass(para[Parameter.mass]) self.chassis_np = self.node_path.attachNewNode(chassis) # not random born now self.chassis_np.setPos(Vec3(*self.born_place, 1)) self.chassis_np.setQuat( LQuaternionf(np.cos(heading / 2), 0, 0, np.sin(heading / 2))) chassis.setDeactivationEnabled(False) chassis.notifyCollisions(True) # advance collision check self.pg_world.physics_world.dynamic_world.setContactAddedCallback( PythonCallbackObject(self._collision_check)) self.dynamic_nodes.append(chassis) chassis_beneath = BulletGhostNode(BodyName.Ego_vehicle) chassis_beneath.setIntoCollideMask(BitMask32.bit(self.COLLISION_MASK)) chassis_beneath.addShape(chassis_shape) self.chassis_beneath_np = self.chassis_np.attachNewNode( chassis_beneath) self.dynamic_nodes.append(chassis_beneath) self.system = BulletVehicle(pg_physics_world.dynamic_world, chassis) self.system.setCoordinateSystem(ZUp) self.dynamic_nodes.append( self.system ) # detach chassis will also detach system, so a waring will generate self.LENGTH = para[Parameter.vehicle_length] self.WIDTH = para[Parameter.vehicle_width] if self.render: model_path = 'models/ferra/scene.gltf' self.chassis_vis = self.loader.loadModel( AssetLoader.file_path(model_path)) self.chassis_vis.setZ(para[Parameter.vehicle_vis_z]) self.chassis_vis.setY(para[Parameter.vehicle_vis_y]) self.chassis_vis.setH(para[Parameter.vehicle_vis_h]) self.chassis_vis.set_scale(para[Parameter.vehicle_vis_scale]) self.chassis_vis.reparentTo(self.chassis_np) def _create_wheel(self): para = self.get_config() f_l = para[Parameter.front_tire_longitude] r_l = -para[Parameter.rear_tire_longitude] lateral = para[Parameter.tire_lateral] axis_height = para[Parameter.tire_radius] + 0.05 radius = para[Parameter.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.node_path.attachNewNode("wheel") if self.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)) # TODO add them to PGConfig in the future wheel.setWheelRadius(radius) wheel.setMaxSuspensionTravelCm(40) wheel.setSuspensionStiffness(30) wheel.setWheelsDampingRelaxation(4.8) wheel.setWheelsDampingCompression(1.2) wheel.setFrictionSlip(self.vehicle_config["wheel_friction"]) wheel.setRollInfluence(1.5) return wheel def add_image_sensor(self, name: str, sensor: ImageBuffer): self.image_sensors[name] = sensor def add_lidar(self, laser_num=240, distance=50): self.lidar = Lidar(self.pg_world.render, laser_num, distance) def add_routing_localization(self, show_navi_point: bool): self.routing_localization = RoutingLocalizationModule( self.pg_world, show_navi_point) def update_map_info(self, map): """ Update map info after reset() :param map: new map :return: None """ self.routing_localization.update(map) lane, new_l_index = ray_localization((self.born_place), self.pg_world) assert lane is not None, "Born place is not on road!" self.lane_index = new_l_index self.lane = lane def _state_check(self): """ Check States and filter to update info """ result = self.pg_world.physics_world.dynamic_world.contactTest( self.chassis_beneath_np.node(), True) contacts = set() for contact in result.getContacts(): node0 = contact.getNode0() node1 = contact.getNode1() name = [node0.getName(), node1.getName()] name.remove(BodyName.Ego_vehicle) if name[0] == "Ground" or name[0] == BodyName.Lane: continue elif name[0] == BodyName.Side_walk: self.out_of_road = True contacts.add(name[0]) if self.render: self.render_collision_info(contacts) return contacts def _collision_check(self, contact): """ It may lower the performance if overdone """ node0 = contact.getNode0().getName() node1 = contact.getNode1().getName() name = [node0, node1] name.remove(BodyName.Ego_vehicle_top) if name[0] == BodyName.Traffic_vehicle: self.crash = True logging.debug("Crash with {}".format(name[0])) def _init_collision_info_render(self, pg_world): if pg_world.mode == "onscreen": info_np = NodePath("Collision info nodepath") info_np.reparentTo(pg_world.aspect2d) else: info_np = None return info_np def render_collision_info(self, contacts): contacts = sorted(list(contacts), key=lambda c: COLLISION_INFO_COLOR[COLOR[c]][0]) text = contacts[0] if len(contacts) != 0 else None if text is None: text = "Normal" if time.time( ) - self.pg_world._episode_start_time > 10 else "Press H to see help message" self.render_banner(text, COLLISION_INFO_COLOR["green"][1]) else: self.render_banner(text, COLLISION_INFO_COLOR[COLOR[text]][1]) def render_banner(self, text, color=COLLISION_INFO_COLOR["green"][1]): """ Render the banner in the left bottom corner. """ if self.collision_info_np is None: return if self.current_banner is not None: self.current_banner.detachNode() if text in self.collision_banners: self.collision_banners[text].reparentTo(self.collision_info_np) self.current_banner = self.collision_banners[text] else: new_banner = NodePath(TextNode("collision_info:{}".format(text))) self.collision_banners[text] = new_banner text_node = new_banner.node() text_node.setCardColor(color) text_node.setText(text) text_node.setCardActual(-5 * self.pg_world.w_scale, 5.1 * self.pg_world.w_scale, -0.3, 1) text_node.setCardDecal(True) text_node.setTextColor(1, 1, 1, 1) text_node.setAlign(TextNode.A_center) new_banner.setScale(0.05) new_banner.setPos(-0.75 * self.pg_world.w_scale, 0, -0.8 * self.pg_world.h_scale) new_banner.reparentTo(self.collision_info_np) self.current_banner = new_banner def destroy(self, _=None): self.dynamic_nodes.remove(self.chassis_np.node()) super(BaseVehicle, self).destroy(self.pg_world) self.pg_world.physics_world.dynamic_world.clearContactAddedCallback() self.routing_localization.destroy() self.routing_localization = 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.pg_world) self.image_sensors = None if self.vehicle_panel is not None: self.vehicle_panel.destroy(self.pg_world) self.pg_world = None def set_position(self, position): """ Should only be called when restore traffic from episode data :param position: 2d array or list :return: None """ self.chassis_np.setPos(panda_position(position, 0.4)) 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.chassis_np.setH((panda_heading(heading_theta) * 180 / np.pi) - 90) def get_state(self): return { "heading": self.heading_theta, "position": self.position.tolist(), "done": self.crash or self.out_of_road } def set_state(self, state: dict): self.set_heading(state["heading"]) self.set_position(state["position"]) def __del__(self): super(BaseVehicle, self).__del__() self.pg_world = None self.lidar = None self.mini_map = None self.rgb_cam = None self.routing_localization = None self.wheels = None
class Roundabout(Block): """ roundabout class, the example is the same as Intersection """ ID = "O" PARAMETER_SPACE = PGSpace(BlockParameterSpace.ROUNDABOUT) SOCKET_NUM = 3 RADIUS_IN = 20 ANGLE = 60 EXIT_PART_LENGTH = 30 def _try_plug_into_previous_block(self) -> bool: para = self.get_config() 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) and no_cross attach_road = -exit_road self.add_reborn_roads( [socket.negative_road for socket in self._sockets]) 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 = sharpbend(right_lane, 10, radius_exit, np.deg2rad(angle), True, self.lane_width, (LineType.STRIPED, 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) 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 = sharpbend( tool_lane, 10, radius_big, np.deg2rad(2 * angle - 90), False, self.lane_width, (LineType.STRIPED, 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) and none_cross # 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 = sharpbend(tool_lane, length, radius_exit, np.deg2rad(angle), True, self.lane_width, (LineType.STRIPED, 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) 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) 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 = np.cos(np.deg2rad(angle)) radius_this_seg = beneath / cos - radius_exit bend, _ = sharpbend(tool_lane, 5, radius_this_seg, np.deg2rad(180 - 2 * angle), False, self.lane_width, (LineType.STRIPED, LineType.SIDE)) CreateRoadFrom(bend, self.positive_lane_num, segment_road, self.block_network, self._global_network) # 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.STRIPED] else: lane.line_types = [LineType.CONTINUOUS, LineType.NONE] else: lane.line_types = [LineType.STRIPED, LineType.STRIPED] return Road(exit_start, exit_end), none_cross def get_socket(self, index: int) -> BlockSocket: socket = super(Roundabout, self).get_socket(index) self._reborn_roads.remove(socket.negative_road) return socket
class Element: PARAMETER_SPACE = PGSpace({}) def __init__(self, random_seed=None): """ Config is a static conception, which specified the parameters of one element. There parameters doesn't change, such as length of straight road, max speed of one vehicle, etc. """ assert isinstance( self.PARAMETER_SPACE, PGSpace ) or random_seed is None, "Using PGSpace to define parameter spaces of " + self.class_name self._config = PGConfig( {k: None for k in self.PARAMETER_SPACE.parameters}) self.random_seed = 0 if random_seed is None else random_seed if self.PARAMETER_SPACE is not None: self.PARAMETER_SPACE.seed(self.random_seed) # Temporally store bullet nodes that have to place in bullet world (not NodePath) self.dynamic_nodes = PhysicsNodeList() # Nodes in this tuple didn't interact with other nodes! they only used to do rayTest or sweepTest self.static_nodes = PhysicsNodeList() self.render = False if AssetLoader.loader is None else True self.node_path = None # each element has its node_path to render, physics node are child nodes of it if self.render: self.loader = AssetLoader.get_loader() @property def class_name(self): return self.__class__.__name__ def get_config(self): assert self._config is not None, "config of " + self.class_name + " is None, can not be read !" return copy.copy(self._config) def set_config(self, config: dict): # logging.debug("Read config to " + self.class_name) self._config.update(copy.copy(config)) def attach_to_pg_world(self, parent_node_path: NodePath, pg_physics_world: PGPhysicsWorld): if self.render: # double check :-) assert isinstance( self.node_path, NodePath), "No render model on node_path in this Element" self.node_path.reparentTo(parent_node_path) self.dynamic_nodes.attach_to_physics_world( pg_physics_world.dynamic_world) self.static_nodes.attach_to_physics_world( pg_physics_world.static_world) def detach_from_pg_world(self, pg_physics_world: PGPhysicsWorld): """ It is not fully remove, if this element is useless in the future, call Func delete() """ self.node_path.detachNode() self.dynamic_nodes.detach_from_physics_world( pg_physics_world.dynamic_world) self.static_nodes.detach_from_physics_world( pg_physics_world.static_world) def destroy(self, pg_world): """ Fully delete this element and release the memory """ self.detach_from_pg_world(pg_world.physics_world) self.node_path.removeNode() self.dynamic_nodes.clear() self.static_nodes.clear() self._config.clear() def __del__(self): pass
class InterSection(Block): """ 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 = PGSpace(BlockParameterSpace.INTERSECTION) SOCKET_NUM = 3 ANGLE = 90 # may support other angle in the future EXIT_PART_LENGTH = 30 # 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 ) and no_cross no_cross = CreateAdverseRoad(exit_road, self.block_network, self._global_network) and no_cross socket = BlockSocket(exit_road, -exit_road) self.add_reborn_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) # 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 = sharpbend( 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)) 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 ) intersect_nodes.rotate(-1) return right_straight, non_cross def get_socket(self, index: int) -> BlockSocket: socket = super(InterSection, self).get_socket(index) self._reborn_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 = sharpbend( 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 ) 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 ) else: left_bend, _ = sharpbend( 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 )
class TInterSection(InterSection): """ A structure like X Intersection, code copied from it mostly """ ID = "T" SOCKET_NUM = 2 PARAMETER_SPACE = PGSpace(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._sockets[(t_type + 1) % 4] next_positive = next_part_socket.positive_road next_negative = next_part_socket.negative_road last_part_socket = self._sockets[(t_type + 3) % 4] 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.STRIPED, LineType.STRIPED ] if k != len(lanes) - 1 else [LineType.STRIPED, 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[t_type].negative_road.end_node end_node = self._sockets[t_type].positive_road.start_node for i in range(4): if i == t_type: continue exit_node = self._sockets[ i].positive_road.start_node if i != Goal.ADVERSE else self._sockets[ i].negative_road.start_node pos_lanes = self.block_network.remove_all_roads( start_node, exit_node) entry_node = self._sockets[ i].negative_road.end_node if i != Goal.ADVERSE else self._sockets[ i].positive_road.end_node neg_lanes = self.block_network.remove_all_roads( entry_node, end_node) # TODO small vis bug may raise in a corner case, # these code can fix it but will introduce a new get_closest_lane_index bug # 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(-1) socket = self._sockets.pop(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._reborn_roads.remove(socket.negative_road)