def set_heading(self, heading_theta) -> None:
"""
Should only be called when restore traffic from episode data
:param heading_theta: float in rad
:return: None
"""
self.origin.setH((panda_heading(heading_theta) * 180 / np.pi) - 90)
def 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 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 _heading_of_lane(lane, pos: Tuple) -> float:
"""
Calculate the heading of a position on lane
:param lane: Abstract lane
:param pos: Tuple, PGDrive coordinates
:return: heading theta
"""
heading_theta = panda_heading(
lane.heading_at(lane.local_coordinates(pos)[0]))
return heading_theta
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 __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)
