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 processInput(self, dt): force = Vec3(0, 0, 0) throttleChange = 0.0 #torque = Vec3(0, 0, 0) if inputState.isSet('forward'): force.setX(1) if inputState.isSet('reverse'): force.setX(-1) if inputState.isSet('left'): force.setY(-1) if inputState.isSet('right'): force.setY(1) if inputState.isSet('turnLeft'): throttleChange = -1.0 if inputState.isSet('turnRight'): throttleChange = 1.0 force *= -20.0 / 180.0 * math.pi self.throttle += throttleChange / 100.0 self.throttle = min(max(self.throttle, 0), 1) thrust = Vec3(0, 0, self.throttle * 50) quatGimbal = self.rocketNozzle.getTransform(self.worldNP).getQuat() self.rocketNozzle.node().applyForce(quatGimbal.xform(thrust), LPoint3f(0, 0, 0)) #torque *= 10.0 #force = render.getRelativeVector(self.rocketNP, force) #torque = render.getRelativeVector(self.rocketNP, torque) self.rocketNozzle.node().setActive(True) self.rocketNP.node().setActive(True) #self.rocketNozzle.node().applyCentralForce(force) #self.rocketNozzle.node().applyTorque(torque) force = rot.from_euler('zyx', force).as_quat() self.cone.setMotorTarget( LQuaternionf(force[0], force[1], force[2], force[3]))
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 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)
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 rotateObject(self, task): if (self.state != State.Playing): return task.cont unitQuaternion = LQuaternionf(1., 0., 0., 0.) currentQuaternion = self.logger.getCurrentQuaternion() # diffQuat = (unitQuaternion - currentQuaternion) hpr = currentQuaternion.get_hpr() # hpr = hpr * 2 # TODO Remove this, this is just for effect. self.model.setHpr(hpr) return Task.cont
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 _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 step(self, time): if self.time is None: self.integrator.set_initial_value(self.state_vector, time) self.time = time else: self.integrator.integrate(time) self.time = self.integrator.t self.state_vector = self.integrator.y self.node_path.setPos(*self.x) self.node_path.setQuat(LQuaternionf(*self.q))
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 __init__(self, pg_world, show_navi_point: False): """ It now only support from first block start to the end node, but can be extended easily """ self.map = None self.final_road = None self.checkpoints = None self.final_lane = None self.target_checkpoints_index = None self.navi_info = None # navi information res self.current_ref_lanes = None # Vis self.showing = True # store the state of navigation mark self.show_navi_point = pg_world.mode == RENDER_MODE_ONSCREEN and not pg_world.pg_config[ "debug_physics_world"] self.goal_node_path = pg_world.render.attachNewNode( "target") if self.show_navi_point else None self.arrow_node_path = pg_world.aspect2d.attachNewNode( "arrow") if self.show_navi_point else None if self.show_navi_point: navi_arrow_model = AssetLoader.loader.loadModel( AssetLoader.file_path("models", "navi_arrow.gltf")) navi_arrow_model.setScale(0.1, 0.12, 0.2) navi_arrow_model.setPos(2, 1.15, -0.221) self.left_arrow = self.arrow_node_path.attachNewNode("left arrow") self.left_arrow.setP(180) self.right_arrow = self.arrow_node_path.attachNewNode( "right arrow") self.left_arrow.setColor(self.MARK_COLOR) self.right_arrow.setColor(self.MARK_COLOR) navi_arrow_model.instanceTo(self.left_arrow) navi_arrow_model.instanceTo(self.right_arrow) self.arrow_node_path.setPos(0, 0, 0.08) self.arrow_node_path.hide(BitMask32.allOn()) self.arrow_node_path.show(CamMask.MainCam) self.arrow_node_path.setQuat( LQuaternionf(np.cos(-np.pi / 4), 0, 0, np.sin(-np.pi / 4))) # the transparency attribute of gltf model is invalid on windows # self.arrow_node_path.setTransparency(TransparencyAttrib.M_alpha) if show_navi_point: navi_point_model = AssetLoader.loader.loadModel( AssetLoader.file_path("models", "box.egg")) navi_point_model.reparentTo(self.goal_node_path) self.goal_node_path.setTransparency(TransparencyAttrib.M_alpha) self.goal_node_path.setColor(0.6, 0.8, 0.5, 0.7) self.goal_node_path.hide(BitMask32.allOn()) self.goal_node_path.show(CamMask.MainCam) logging.debug("Load Vehicle Module: {}".format( self.__class__.__name__))
def _add_chassis(self, pg_physics_world: PGPhysicsWorld): para = self.get_config() self.LENGTH = self.vehicle_config["vehicle_length"] self.WIDTH = self.vehicle_config["vehicle_width"] chassis = BaseVehicleNode(BodyName.Base_vehicle, self) chassis.setIntoCollideMask(BitMask32.bit(CollisionGroup.EgoVehicle)) chassis_shape = BulletBoxShape( Vec3(self.WIDTH / 2, self.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 spawn now self.chassis_np.setPos(Vec3(*self.spawn_place, 1)) self.chassis_np.setQuat( LQuaternionf(math.cos(heading / 2), 0, 0, math.sin(heading / 2))) chassis.setDeactivationEnabled(False) chassis.notifyCollisions( True ) # advance collision check, do callback in pg_collision_callback self.dynamic_nodes.append(chassis) chassis_beneath = BulletGhostNode(BodyName.Base_vehicle_beneath) chassis_beneath.setIntoCollideMask( BitMask32.bit(CollisionGroup.EgoVehicleBeneath)) 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 if self.render: if self.MODEL is None: model_path = 'models/ferra/scene.gltf' self.MODEL = self.loader.loadModel( AssetLoader.file_path(model_path)) self.MODEL.setZ(para[Parameter.vehicle_vis_z]) self.MODEL.setY(para[Parameter.vehicle_vis_y]) self.MODEL.setH(para[Parameter.vehicle_vis_h]) self.MODEL.set_scale(para[Parameter.vehicle_vis_scale]) self.MODEL.instanceTo(self.chassis_np)
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 _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 create_scene(self, directory): self.__directory = directory loadPrcFileData("", "model-path %s" % directory) self.__scene = FruitConfigParser() self.__scene.read(self.__path("scene.cfg")) for thing in self.__scene.get("scene", "things").split(" "): egg = self.__scene.get(thing, "egg") egg = self.loader.loadModel(egg) egg.reparentTo(self.render) egg.setPos(*self.__scene.getfloats(thing, "location")) egg.setQuat(LQuaternionf(*self.__scene.getfloats(thing, "rotation"))) egg.setScale(*self.__scene.getfloats(thing, "scale")) if self.__scene.has_option("scene", "stencil"): self.__create_terrain() if self.__scene.has_option("scene", "skybox"): self.__create_skybox()
def _add_box_body(self, lane_start, lane_end, middle, parent_np: NodePath, line_type): length = norm(lane_end[0] - lane_start[0], lane_end[1] - lane_start[1]) if LineType.prohibit(line_type): node_name = BodyName.Continuous_line else: node_name = BodyName.Stripped_line body_node = BulletRigidBodyNode(node_name) body_node.setActive(False) body_node.setKinematic(False) body_node.setStatic(True) body_np = parent_np.attachNewNode(body_node) shape = BulletBoxShape(Vec3(length / 2, Block.LANE_LINE_WIDTH / 2, Block.LANE_LINE_THICKNESS)) body_np.node().addShape(shape) body_np.node().setIntoCollideMask(BitMask32.bit(Block.LANE_LINE_COLLISION_MASK)) self.dynamic_nodes.append(body_np.node()) body_np.setPos(panda_position(middle, 0)) direction_v = lane_end - lane_start theta = -numpy.arctan2(direction_v[1], direction_v[0]) body_np.setQuat(LQuaternionf(numpy.cos(theta / 2), 0, 0, numpy.sin(theta / 2)))
def __init__(self): super(Terrain, self).__init__(random_seed=0) shape = BulletPlaneShape(Vec3(0, 0, 1), 0) node = BulletRigidBodyNode(BodyName.Ground) node.setFriction(.9) node.addShape(shape) node.setIntoCollideMask(self.COLLISION_MASK) self.dynamic_nodes.append(node) self.origin.attachNewNode(node) if self.render: self.origin.hide(CamMask.MiniMap | CamMask.Shadow | CamMask.DepthCam | CamMask.ScreenshotCam) # self.terrain_normal = self.loader.loadTexture( # AssetLoader.file_path( "textures", "grass2", "normal.jpg") # ) self.terrain_texture = self.loader.loadTexture( AssetLoader.file_path("textures", "ground.png")) self.terrain_texture.setWrapU(Texture.WM_repeat) self.terrain_texture.setWrapV(Texture.WM_repeat) self.ts_color = TextureStage("color") self.ts_normal = TextureStage("normal") self.ts_normal.set_mode(TextureStage.M_normal) self.origin.setPos(0, 0, self.HEIGHT) cm = CardMaker('card') scale = 20000 cm.setUvRange((0, 0), (scale / 10, scale / 10)) card = self.origin.attachNewNode(cm.generate()) # scale = 1 if self.use_hollow else 20000 card.set_scale(scale) card.setPos(-scale / 2, -scale / 2, -0.1) card.setZ(-.05) card.setTexture(self.ts_color, self.terrain_texture) # card.setTexture(self.ts_normal, self.terrain_normal) self.terrain_texture.setMinfilter( SamplerState.FT_linear_mipmap_linear) self.terrain_texture.setAnisotropicDegree(8) card.setQuat( LQuaternionf(math.cos(-math.pi / 4), math.sin(-math.pi / 4), 0, 0))
def _render_with_pose(self, R: np.ndarray, t: np.ndarray) -> np.ndarray: R = self._convert_R(R) t = self._convert_t(t) # Set the camera position. # SciPy quaternions are (x, y, z, w), but Panda3D quaternions are (w, x, y, z). #nice q = Rotation.from_matrix(R).as_quat() self.camera.setQuat(LQuaternionf(q[3], q[0], q[1], q[2])) self.camera.setPos(t[0], t[1], t[2]) # Get a frame from Panda3D. self.graphicsEngine.renderFrame() self.graphicsEngine.renderFrame() display_region = self.camNode.getDisplayRegion(0) tex = display_region.getScreenshot() data = tex.getRamImage() image = np.frombuffer(data, np.uint8) image.shape = (tex.getYSize(), tex.getXSize(), tex.getNumComponents()) # Copy the image to a numpy array. # Relative to the Panda3D viewer, the image is upside down, so it needs to be flipped. image_np = np.empty(self.image_shape, dtype=np.uint8) np.copyto(image_np, image[:, :, 0:3]) return np.flip(image_np, 0)
def rotateActorTask(self, task): global qrb q = LQuaternionf(qrb.q[0], qrb.q[1], qrb.q[2], qrb.q[3]) self.quadrotor.setQuat(q) return Task.cont
def __init__(self, pg_world, show_navi_mark: bool = False, random_navi_mark_color=False, show_dest_mark=False, show_line_to_dest=False): """ This class define a helper for localizing vehicles and retrieving navigation information. It now only support from first block start to the end node, but can be extended easily. """ self.map = None self.final_road = None self.checkpoints = None self.final_lane = None self.current_ref_lanes = None self.current_road = None self._target_checkpoints_index = None self._navi_info = np.zeros( (self.navigation_info_dim, )) # navi information res self.navi_mark_color = ( 0.6, 0.8, 0.5) if not random_navi_mark_color else get_np_random().rand(3) # Vis self._is_showing = True # store the state of navigation mark self._show_navi_info = ( pg_world.mode == RENDER_MODE_ONSCREEN and not pg_world.world_config["debug_physics_world"]) self._dest_node_path = None self._goal_node_path = None self._arrow_node_path = None self._line_to_dest = None self._show_line_to_dest = show_line_to_dest if self._show_navi_info: # nodepath self._line_to_dest = pg_world.render.attachNewNode("line") self._goal_node_path = pg_world.render.attachNewNode("target") self._dest_node_path = pg_world.render.attachNewNode("dest") self._arrow_node_path = pg_world.aspect2d.attachNewNode("arrow") navi_arrow_model = AssetLoader.loader.loadModel( AssetLoader.file_path("models", "navi_arrow.gltf")) navi_arrow_model.setScale(0.1, 0.12, 0.2) navi_arrow_model.setPos(2, 1.15, -0.221) self._left_arrow = self._arrow_node_path.attachNewNode( "left arrow") self._left_arrow.setP(180) self._right_arrow = self._arrow_node_path.attachNewNode( "right arrow") self._left_arrow.setColor(self.MARK_COLOR) self._right_arrow.setColor(self.MARK_COLOR) navi_arrow_model.instanceTo(self._left_arrow) navi_arrow_model.instanceTo(self._right_arrow) self._arrow_node_path.setPos(0, 0, 0.08) self._arrow_node_path.hide(BitMask32.allOn()) self._arrow_node_path.show(CamMask.MainCam) self._arrow_node_path.setQuat( LQuaternionf(np.cos(-np.pi / 4), 0, 0, np.sin(-np.pi / 4))) # the transparency attribute of gltf model is invalid on windows # self._arrow_node_path.setTransparency(TransparencyAttrib.M_alpha) if show_navi_mark: navi_point_model = AssetLoader.loader.loadModel( AssetLoader.file_path("models", "box.bam")) navi_point_model.reparentTo(self._goal_node_path) if show_dest_mark: dest_point_model = AssetLoader.loader.loadModel( AssetLoader.file_path("models", "box.bam")) dest_point_model.reparentTo(self._dest_node_path) if show_line_to_dest: line_seg = LineSegs("line_to_dest") line_seg.setColor(self.navi_mark_color[0], self.navi_mark_color[1], self.navi_mark_color[2], 0.7) line_seg.setThickness(2) self._dynamic_line_np = NodePath(line_seg.create(True)) self._dynamic_line_np.reparentTo(pg_world.render) self._line_to_dest = line_seg self._goal_node_path.setTransparency(TransparencyAttrib.M_alpha) self._dest_node_path.setTransparency(TransparencyAttrib.M_alpha) self._goal_node_path.setColor(self.navi_mark_color[0], self.navi_mark_color[1], self.navi_mark_color[2], 0.7) self._dest_node_path.setColor(self.navi_mark_color[0], self.navi_mark_color[1], self.navi_mark_color[2], 0.7) self._goal_node_path.hide(BitMask32.allOn()) self._dest_node_path.hide(BitMask32.allOn()) self._goal_node_path.show(CamMask.MainCam) self._dest_node_path.show(CamMask.MainCam) logging.debug("Load Vehicle Module: {}".format( self.__class__.__name__))
def CreateQuaternion(stringQuat): return LQuaternionf(float(stringQuat[0]), float(stringQuat[1]), float(stringQuat[2]), float(stringQuat[3]))
def moveActorTask(self, task): pos = self.qrb.pos q = LQuaternionf(self.qrb.q[0],self.qrb.q[1],self.qrb.q[2],self.qrb.q[3]) self.quadrotor.setQuat(q) self.quadrotor.setPos(pos[0],pos[1],pos[2]) return Task.cont