def circle_region_detection(engine: EngineCore, position: Tuple, radius: float, detection_group: int, height=10, in_static_world=False): """ :param engine: BaseEngine class :param position: position in PGDrive :param radius: radius of the region to be detected :param detection_group: which group to detect :param height: the detect will be executed from this height to 0 :param in_static_world: execute detection in static world :return: detection result """ region_detect_start = panda_position(position, z=height) region_detect_end = panda_position(position, z=-1) tsFrom = TransformState.makePos(region_detect_start) tsTo = TransformState.makePos(region_detect_end) shape = BulletCylinderShape(radius, 5, ZUp) penetration = 0.0 physics_world = engine.physics_world.dynamic_world if not in_static_world else engine.physics_world.static_world result = physics_world.sweep_test_closest(shape, tsFrom, tsTo, detection_group, penetration) return result
def perceive(self, vehicle_position, heading_theta, pg_physics_world: PGPhysicsWorld): """ Call me to update the perception info """ # coordinates problem here! take care pg_start_position = panda_position(vehicle_position, 1.0) self.detection_results = [] # lidar calculation use pg coordinates mask = BitMask32.bit(PGTrafficVehicle.COLLISION_MASK) laser_heading = np.arange(0, self.laser_num) * self.radian_unit + heading_theta point_x = self.perceive_distance * np.cos(laser_heading) + vehicle_position[0] point_y = self.perceive_distance * np.sin(laser_heading) + vehicle_position[1] # ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ for laser_index in range(self.laser_num): # # coordinates problem here! take care laser_end = panda_position((point_x[laser_index], point_y[laser_index]), 1.0) result = pg_physics_world.dynamic_world.rayTestClosest(pg_start_position, laser_end, mask) self.detection_results.append(result) if self.cloud_points is not None: if result.hasHit(): curpos = result.getHitPos() else: curpos = laser_end self.cloud_points[laser_index].setPos(curpos)
def _draw_line_to_dest(self, engine, start_position, end_position): if not self._show_line_to_dest: return line_seg = self._line_to_dest line_seg.moveTo( panda_position(start_position, self.LINE_TO_DEST_HEIGHT)) line_seg.drawTo(panda_position(end_position, self.LINE_TO_DEST_HEIGHT)) self._dynamic_line_np.removeNode() self._dynamic_line_np = NodePath(line_seg.create(False)) self._dynamic_line_np.hide(CamMask.Shadow | CamMask.RgbCam) self._dynamic_line_np.reparentTo(self.origin)
def ray_localization(heading: tuple, position: tuple, engine: EngineCore, return_all_result=False) -> Union[List[Tuple], Tuple]: """ Get the index of the lane closest to a physx_world position. Only used when smoething is on lane ! Otherwise fall back to use get_closest_lane() :param heading: heading to help filter lanes :param position: a physx_world position [m]. :param engine: PGWorld class :param return_all_result: return a list instead of the lane with min L1 distance :return: list(closest lane) or closest lane. """ results = engine.physics_world.static_world.rayTestAll( panda_position(position, 1.0), panda_position(position, -1.0)) lane_index_dist = [] if results.hasHits(): for res in results.getHits(): if res.getNode().getName() == BodyName.Lane: lane = get_object_from_node(res.getNode()) long, _ = lane.local_coordinates(position) lane_heading = lane.heading_at(long) # dir = np.array([math.cos(lane_heading), math.sin(lane_heading)]) # dot_result = dir.dot(heading) dot_result = math.cos(lane_heading) * heading[0] + math.sin( lane_heading) * heading[1] cosangle = dot_result / ( norm(math.cos(lane_heading), math.sin(lane_heading)) * norm(heading[0], heading[1])) if cosangle > 0: lane_index_dist.append( (lane, lane.index, lane.distance(position))) if return_all_result: ret = [] if len(lane_index_dist) > 0: for lane, index, dist in lane_index_dist: ret.append((lane, index, dist)) sorted(ret, key=lambda k: k[2]) return ret else: if len(lane_index_dist) > 0: ret_index = np.argmin([d for _, _, d in lane_index_dist]) lane, index, dist = lane_index_dist[ret_index] else: lane, index, dist = None, None, None return lane, index
def set_position(self, position): """ Should only be called when restore traffic from episode data :param position: 2d array or list :return: None """ self.node_path.setPos(panda_position(position, 0))
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 _add_side_walk2bullet(self, lane_start, lane_end, middle, radius=0.0, direction=0): length = norm(lane_end[0] - lane_start[0], lane_end[1] - lane_start[1]) body_node = BulletRigidBodyNode(BodyName.Side_walk) body_node.setActive(False) body_node.setKinematic(False) body_node.setStatic(True) side_np = self.side_walk_node_path.attachNewNode(body_node) shape = BulletBoxShape(Vec3(1 / 2, 1 / 2, 1 / 2)) body_node.addShape(shape) body_node.setIntoCollideMask(BitMask32.bit(Block.LANE_LINE_COLLISION_MASK)) self.dynamic_nodes.append(body_node) if radius == 0: factor = 1 else: if direction == 1: factor = (1 - self.SIDE_WALK_LINE_DIST / radius) else: factor = (1 + self.SIDE_WALK_WIDTH / radius) * (1 + self.SIDE_WALK_LINE_DIST / radius) direction_v = lane_end - lane_start vertical_v = (-direction_v[1], direction_v[0]) / numpy.linalg.norm(direction_v) middle += vertical_v * (self.SIDE_WALK_WIDTH / 2 + self.SIDE_WALK_LINE_DIST) side_np.setPos(panda_position(middle, 0)) theta = -numpy.arctan2(direction_v[1], direction_v[0]) side_np.setQuat(LQuaternionf(numpy.cos(theta / 2), 0, 0, numpy.sin(theta / 2))) side_np.setScale( length * factor, self.SIDE_WALK_WIDTH, self.SIDE_WALK_THICKNESS * (1 + 0.1 * numpy.random.rand()) ) if self.render: side_np.setTexture(self.ts_color, self.side_texture) self.side_walk.instanceTo(side_np)
def _add_box_body(self, lane_start, lane_end, middle, parent_np: NodePath, line_type, line_color): length = norm(lane_end[0] - lane_start[0], lane_end[1] - lane_start[1]) if LineType.prohibit(line_type): node_name = BodyName.White_continuous_line if line_color == LineColor.GREY else BodyName.Yellow_continuous_line else: node_name = BodyName.Broken_line body_node = BulletGhostNode(node_name) body_node.set_active(False) body_node.setKinematic(False) body_node.setStatic(True) body_np = parent_np.attachNewNode(body_node) shape = BulletBoxShape( Vec3(length / 2, Block.LANE_LINE_WIDTH / 2, Block.LANE_LINE_GHOST_HEIGHT)) body_np.node().addShape(shape) mask = Block.CONTINUOUS_COLLISION_MASK if line_type != LineType.BROKEN else Block.BROKEN_COLLISION_MASK body_np.node().setIntoCollideMask(BitMask32.bit(mask)) self.static_nodes.append(body_np.node()) body_np.setPos(panda_position(middle, Block.LANE_LINE_GHOST_HEIGHT / 2)) direction_v = lane_end - lane_start theta = -numpy.arctan2(direction_v[1], direction_v[0]) body_np.setQuat( LQuaternionf(numpy.cos(theta / 2), 0, 0, numpy.sin(theta / 2)))
def _get_laser_end(self, laser_index, heading_theta, vehicle_position): point_x = self.perceive_distance * math.cos(self._lidar_range[laser_index] + heading_theta) + \ vehicle_position[0] point_y = self.perceive_distance * math.sin(self._lidar_range[laser_index] + heading_theta) + \ vehicle_position[1] laser_end = panda_position((point_x, point_y), self.height) return laser_end
def step(self, dt): self.vehicle_node.kinematic_model.step(dt) position = panda_position(self.vehicle_node.kinematic_model.position, 0) self.node_path.setPos(position) heading = np.rad2deg( panda_heading(self.vehicle_node.kinematic_model.heading)) self.node_path.setH(heading)
def rect_region_detection(engine: EngineCore, position: Tuple, heading: float, heading_direction_length: float, side_direction_width: float, detection_group: int, height=10, in_static_world=False): """ ---------------------------------- | * | --->>> ---------------------------------- * position --->>> heading direction ------ longitude length | lateral width **CAUTION**: position is the middle point of longitude edge :param engine: BaseEngine class :param position: position in PGDrive :param heading: heading in PGDrive [degree] :param heading_direction_length: rect length in heading direction :param side_direction_width: rect width in side direction :param detection_group: which group to detect :param height: the detect will be executed from this height to 0 :param in_static_world: execute detection in static world :return: detection result """ region_detect_start = panda_position(position, z=height) region_detect_end = panda_position(position, z=-1) tsFrom = TransformState.makePosHpr(region_detect_start, Vec3(panda_heading(heading), 0, 0)) tsTo = TransformState.makePosHpr(region_detect_end, Vec3(panda_heading(heading), 0, 0)) shape = BulletBoxShape( Vec3(heading_direction_length / 2, side_direction_width / 2, 1)) penetration = 0.0 physics_world = engine.physics_world.dynamic_world if not in_static_world else engine.physics_world.static_world result = physics_world.sweep_test_closest(shape, tsFrom, tsTo, detection_group, penetration) return result
def _add_lane2bullet(self, middle, width, length, theta, lane: Union[StraightLane, CircularLane], lane_index): """ Add lane visualization and body for it :param middle: Middle point :param width: Lane width :param length: Segment length :param theta: Rotate theta :param lane: Lane info :return: None """ segment_np = NodePath(LaneNode(BodyName.Lane, lane, lane_index)) segment_node = segment_np.node() segment_node.set_active(False) segment_node.setKinematic(False) segment_node.setStatic(True) shape = BulletBoxShape(Vec3(length / 2, 0.1, width / 2)) segment_node.addShape(shape) self.static_nodes.append(segment_node) segment_np.setPos(panda_position(middle, -0.1)) segment_np.setQuat( LQuaternionf( numpy.cos(theta / 2) * numpy.cos(-numpy.pi / 4), numpy.cos(theta / 2) * numpy.sin(-numpy.pi / 4), -numpy.sin(theta / 2) * numpy.cos(-numpy.pi / 4), numpy.sin(theta / 2) * numpy.cos(-numpy.pi / 4))) segment_np.reparentTo(self.lane_node_path) if self.render: cm = CardMaker('card') cm.setFrame(-length / 2, length / 2, -width / 2, width / 2) cm.setHasNormals(True) cm.setUvRange((0, 0), (length / 20, width / 10)) card = self.lane_vis_node_path.attachNewNode(cm.generate()) card.setPos( panda_position(middle, numpy.random.rand() * 0.01 - 0.01)) card.setQuat( LQuaternionf( numpy.cos(theta / 2) * numpy.cos(-numpy.pi / 4), numpy.cos(theta / 2) * numpy.sin(-numpy.pi / 4), -numpy.sin(theta / 2) * numpy.cos(-numpy.pi / 4), numpy.sin(theta / 2) * numpy.cos(-numpy.pi / 4))) card.setTransparency(TransparencyAttrib.MMultisample) card.setTexture(self.ts_color, self.road_texture)
def get_available_respawn_places(self, map, randomize=False): """ In each episode, we allow the vehicles to respawn at the start of road, randomize will give vehicles a random position in the respawn region """ engine = get_engine() ret = {} for bid, bp in self.safe_spawn_places.items(): if bid in self.spawn_places_used: continue # save time calculate once if not bp.get("spawn_point_position", False): lane = map.road_network.get_lane( bp["config"]["spawn_lane_index"]) assert isinstance( lane, StraightLane ), "Now we don't support respawn on circular lane" long = self.RESPAWN_REGION_LONGITUDE / 2 spawn_point_position = lane.position(longitudinal=long, lateral=0) bp.force_update({ "spawn_point_heading": np.rad2deg(lane.heading_at(long)), "spawn_point_position": (spawn_point_position[0], spawn_point_position[1]) }) spawn_point_position = bp["spawn_point_position"] lane_heading = bp["spawn_point_heading"] result = rect_region_detection(engine, spawn_point_position, lane_heading, self.RESPAWN_REGION_LONGITUDE, self.RESPAWN_REGION_LATERAL, CollisionGroup.Vehicle) if (engine.global_config["debug"] or engine.global_config["debug_physics_world"]) \ and bp.get("need_debug", True): shape = BulletBoxShape( Vec3(self.RESPAWN_REGION_LONGITUDE / 2, self.RESPAWN_REGION_LATERAL / 2, 1)) vis_body = engine.render.attach_new_node( BulletGhostNode("debug")) vis_body.node().addShape(shape) vis_body.setH(panda_heading(lane_heading)) vis_body.setPos(panda_position(spawn_point_position, z=2)) engine.physics_world.dynamic_world.attach(vis_body.node()) vis_body.node().setIntoCollideMask(CollisionGroup.AllOff) bp.force_set("need_debug", False) if not result.hasHit(): new_bp = copy.deepcopy(bp).get_dict() if randomize: new_bp["config"] = self._randomize_position_in_slot( new_bp["config"]) ret[bid] = new_bp self.spawn_places_used.append(bid) return ret
def set_bird_view_pos(self, position): if self.engine.task_manager.hasTaskNamed(self.TOP_DOWN_TASK_NAME): # adjust hpr p_pos = panda_position(position) self.camera_x, self.camera_y = p_pos[0], p_pos[1] self.engine.task_manager.add(self._top_down_task, self.TOP_DOWN_TASK_NAME, extraArgs=[], appendTask=True)
def _ray_lateral_range(self, pg_world, start_position, dir, length=50): """ It is used to measure the lateral range of special blocks :param start_position: start_point :param dir: ray direction :param length: length of ray :return: lateral range [m] """ end_position = start_position[0] + dir[0] * length, start_position[ 1] + dir[1] * length start_position = panda_position(start_position, z=0.15) end_position = panda_position(end_position, z=0.15) mask = BitMask32.bit(FirstBlock.CONTINUOUS_COLLISION_MASK) res = pg_world.physics_world.static_world.rayTestClosest( start_position, end_position, mask=mask) if not res.hasHit(): return length else: return res.getHitFraction() * length
def _add_lane_line2bullet(self, lane_start, lane_end, middle, parent_np: NodePath, color: Vec4, line_type: LineType, straight_stripe=False): length = norm(lane_end[0] - lane_start[0], lane_end[1] - lane_start[1]) if length <= 0: return if LineType.prohibit(line_type): node_name = BodyName.White_continuous_line if color == LineColor.GREY else BodyName.Yellow_continuous_line else: node_name = BodyName.Broken_line # add bullet body for it if straight_stripe: body_np = parent_np.attachNewNode(node_name) else: body_node = BulletGhostNode(node_name) body_node.set_active(False) body_node.setKinematic(False) body_node.setStatic(True) body_np = parent_np.attachNewNode(body_node) # its scale will change by setScale body_height = DrivableAreaProperty.LANE_LINE_GHOST_HEIGHT shape = BulletBoxShape( Vec3(length / 2 if line_type != LineType.BROKEN else length, DrivableAreaProperty.LANE_LINE_WIDTH / 2, body_height)) body_np.node().addShape(shape) mask = DrivableAreaProperty.CONTINUOUS_COLLISION_MASK if line_type != LineType.BROKEN else DrivableAreaProperty.BROKEN_COLLISION_MASK body_np.node().setIntoCollideMask(mask) self.static_nodes.append(body_np.node()) # position and heading body_np.setPos( panda_position(middle, DrivableAreaProperty.LANE_LINE_GHOST_HEIGHT / 2)) direction_v = lane_end - lane_start # theta = -numpy.arctan2(direction_v[1], direction_v[0]) theta = -math.atan2(direction_v[1], direction_v[0]) body_np.setQuat( LQuaternionf(math.cos(theta / 2), 0, 0, math.sin(theta / 2))) if self.render: # For visualization lane_line = self.loader.loadModel( AssetLoader.file_path("models", "box.bam")) lane_line.setScale(length, DrivableAreaProperty.LANE_LINE_WIDTH, DrivableAreaProperty.LANE_LINE_THICKNESS) lane_line.setPos( Vec3(0, 0 - DrivableAreaProperty.LANE_LINE_GHOST_HEIGHT / 2)) lane_line.reparentTo(body_np) body_np.set_color(color)
def 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
def reset(self, map: Map, 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 pos is None: lane = map.road_network.get_lane( self.vehicle_config["spawn_lane_index"]) pos = lane.position(self.vehicle_config["spawn_longitude"], self.vehicle_config["spawn_lateral"]) heading = np.rad2deg( lane.heading_at(self.vehicle_config["spawn_longitude"])) self.spawn_place = pos heading = -np.deg2rad(heading) - np.pi / 2 self.set_static(False) self.chassis_np.setPos(panda_position(Vec3(*pos, 1))) self.chassis_np.setQuat( LQuaternionf(math.cos(heading / 2), 0, 0, math.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() # 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() # for render if self.vehicle_panel is not None: self.vehicle_panel.renew_2d_car_para_visualization(self) 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) assert self.routing_localization
def ray_localization(position: np.ndarray, pg_world: PGWorld) -> Tuple: """ Get the index of the lane closest to a physx_world position. Only used when smoething is on lane ! Otherwise fall back to use get_closest_lane() :param position: a physx_world position [m]. :param pg_world: PGWorld class :return: the index of the closest lane. """ results = pg_world.physics_world.static_world.rayTestAll( panda_position(position, 1.0), panda_position(position, -1.0)) lane_index_dist = [] if results.hasHits(): for res in results.getHits(): if res.getNode().getName() == BodyName.Lane: lane = res.getNode().getPythonTag(BodyName.Lane) lane_index_dist.append( (lane.info, lane.index, lane.info.distance(position))) if len(lane_index_dist) > 0: ret_index = np.argmin([d for _, _, d in lane_index_dist]) lane, index, dist = lane_index_dist[ret_index] else: lane, index, dist = None, None, None return lane, index
def __init__(self, lane, position, heading, random_seed): super(TollGateBuilding, self).__init__(lane, position, heading, random_seed) air_wall = generate_invisible_static_wall( self.BUILDING_LENGTH, lane.width, self.BUILDING_HEIGHT / 2, object_id=self.id ) self.add_body(air_wall) self.origin.setPos(panda_position(position)) self.origin.setH(panda_heading(heading)) if self.render: building_model = self.loader.loadModel(AssetLoader.file_path("models", "tollgate", "booth.gltf")) gate_model = self.loader.loadModel(AssetLoader.file_path("models", "tollgate", "gate.gltf")) building_model.setH(90) building_model.reparentTo(self.origin) gate_model.reparentTo(self.origin)
def get_surrounding_objects(self, vehicle): self.broad_detector.setPos(panda_position(vehicle.position)) physics_world = vehicle.engine.physics_world.dynamic_world contact_results = physics_world.contactTest(self.broad_detector.node(), True).getContacts() objs = set() for contact in contact_results: node0 = contact.getNode0() node1 = contact.getNode1() nodes = [node0, node1] nodes.remove(self.broad_detector.node()) obj = get_object_from_node(nodes[0]) if not isinstance(obj, AbstractLane) and obj is not None: objs.add(obj) if vehicle in objs: objs.remove(vehicle) return objs
def _add_lane_line2bullet( self, lane_start, lane_end, middle, parent_np: NodePath, color: Vec4, line_type: LineType, straight_stripe=False ): length = norm(lane_end[0] - lane_start[0], lane_end[1] - lane_start[1]) if length <= 0: return if LineType.prohibit(line_type): node_name = BodyName.Continuous_line else: node_name = BodyName.Stripped_line # add bullet body for it if straight_stripe: body_np = parent_np.attachNewNode(node_name) else: scale = 2 if line_type == LineType.STRIPED else 1 body_node = BulletRigidBodyNode(node_name) body_node.setActive(False) body_node.setKinematic(False) body_node.setStatic(True) body_np = parent_np.attachNewNode(body_node) shape = BulletBoxShape(Vec3(scale / 2, Block.LANE_LINE_WIDTH / 2, Block.LANE_LINE_THICKNESS)) body_np.node().addShape(shape) body_np.node().setIntoCollideMask(BitMask32.bit(Block.LANE_LINE_COLLISION_MASK)) self.dynamic_nodes.append(body_np.node()) # position and heading body_np.setPos(panda_position(middle, 0)) direction_v = lane_end - lane_start theta = -numpy.arctan2(direction_v[1], direction_v[0]) body_np.setQuat(LQuaternionf(numpy.cos(theta / 2), 0, 0, numpy.sin(theta / 2))) if self.render: # For visualization lane_line = self.loader.loadModel(AssetLoader.file_path("models", "box.bam")) lane_line.getChildren().reparentTo(body_np) body_np.setScale(length, Block.LANE_LINE_WIDTH, Block.LANE_LINE_THICKNESS) body_np.set_color(color)
def __init__(self, lane, lane_index: LaneIndex, position: Sequence[float], heading: float = 0.): super(TrafficCone, self).__init__(lane, lane_index, position, heading) self.body_node = ObjectNode(self.NAME) self.body_node.addShape(BulletCylinderShape(self.RADIUS, self.HEIGHT)) self.node_path: NodePath = NodePath(self.body_node) self.node_path.setPos(panda_position(self.position, self.HEIGHT / 2)) self.dynamic_nodes.append(self.body_node) self.node_path.setH(panda_heading(self.heading)) if self.render: model = self.loader.loadModel( AssetLoader.file_path("models", "traffic_cone", "scene.gltf")) model.setScale(0.02) model.setPos(0, 0, -self.HEIGHT / 2) model.reparentTo(self.node_path)
def __init__(self, lane, longitude: float, lateral: float, static: bool = False, random_seed=None): super(TrafficCone, self).__init__(lane, longitude, lateral, random_seed) self.add_body(BaseRigidBodyNode(self.name, self.NAME)) self.body.addShape(BulletCylinderShape(self.RADIUS, self.HEIGHT)) self.body.setIntoCollideMask(self.COLLISION_GROUP) self.origin.setPos(panda_position(self.position, self.HEIGHT / 2)) self.origin.setH(panda_heading(self.heading)) if self.render: model = self.loader.loadModel( AssetLoader.file_path("models", "traffic_cone", "scene.gltf")) model.setScale(0.02) model.setPos(0, 0, -self.HEIGHT / 2) model.reparentTo(self.origin) self.set_static(static)
def __init__(self, lane, longitude: float, lateral: float, static: bool = False, random_seed=None): super(TrafficBarrier, self).__init__(lane, longitude, lateral, random_seed) self.add_body(BaseRigidBodyNode(self.name, self.NAME)) self.body.addShape( BulletBoxShape((self.WIDTH / 2, self.LENGTH / 2, self.HEIGHT / 2))) self.body.setIntoCollideMask(self.COLLISION_GROUP) self.origin.setPos(panda_position(self.position, self.HEIGHT / 2)) self.origin.setH(panda_heading(self.heading)) if self.render: model = self.loader.loadModel( AssetLoader.file_path("models", "barrier", "scene.gltf")) model.setH(-90) model.reparentTo(self.origin) self.set_static(static)
def _add_box_body(self, lane_start, lane_end, middle, parent_np: NodePath, line_type): length = norm(lane_end[0] - lane_start[0], lane_end[1] - lane_start[1]) if LineType.prohibit(line_type): node_name = BodyName.Continuous_line else: node_name = BodyName.Stripped_line body_node = BulletRigidBodyNode(node_name) body_node.setActive(False) body_node.setKinematic(False) body_node.setStatic(True) body_np = parent_np.attachNewNode(body_node) shape = BulletBoxShape(Vec3(length / 2, Block.LANE_LINE_WIDTH / 2, Block.LANE_LINE_THICKNESS)) body_np.node().addShape(shape) body_np.node().setIntoCollideMask(BitMask32.bit(Block.LANE_LINE_COLLISION_MASK)) self.dynamic_nodes.append(body_np.node()) body_np.setPos(panda_position(middle, 0)) direction_v = lane_end - lane_start theta = -numpy.arctan2(direction_v[1], direction_v[0]) body_np.setQuat(LQuaternionf(numpy.cos(theta / 2), 0, 0, numpy.sin(theta / 2)))