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)))
