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