def heading_diff(self, target_lane):
lateral = None
if isinstance(target_lane, StraightLane):
lateral = np.asarray(
get_vertical_vector(target_lane.end - target_lane.start)[1])
elif isinstance(target_lane, CircularLane):
if target_lane.direction == -1:
lateral = self.position - target_lane.center
else:
lateral = target_lane.center - self.position
elif isinstance(target_lane, WayPointLane):
lane_segment = target_lane.segment(
target_lane.local_coordinates(self.position)[0])
lateral = lane_segment["lateral_direction"]
lateral_norm = norm(lateral[0], lateral[1])
forward_direction = self.heading
# print(f"Old forward direction: {self.forward_direction}, new heading {self.heading}")
forward_direction_norm = norm(forward_direction[0],
forward_direction[1])
if not lateral_norm * forward_direction_norm:
return 0
cos = ((forward_direction[0] * lateral[0] +
forward_direction[1] * lateral[1]) /
(lateral_norm * forward_direction_norm))
# return cos
# Normalize to 0, 1
return clip(cos, -1.0, 1.0) / 2 + 0.5
def vehicle_state(self, vehicle):
# update out of road
info = []
lateral_to_left, lateral_to_right, = vehicle.dist_to_left_side, vehicle.dist_to_right_side
total_width = float(
(vehicle.routing_localization.get_current_lane_num() + 1) *
vehicle.routing_localization.get_current_lane_width())
lateral_to_left /= total_width
lateral_to_right /= total_width
info += [
clip(lateral_to_left, 0.0, 1.0),
clip(lateral_to_right, 0.0, 1.0)
]
current_reference_lane = vehicle.routing_localization.current_ref_lanes[
-1]
info += [
vehicle.heading_diff(current_reference_lane),
# Note: speed can be negative denoting free fall. This happen when emergency brake.
clip((vehicle.speed + 1) / (vehicle.max_speed + 1), 0.0, 1.0),
clip((vehicle.steering / vehicle.max_steering + 1) / 2, 0.0, 1.0),
clip((vehicle.last_current_action[0][0] + 1) / 2, 0.0, 1.0),
clip((vehicle.last_current_action[0][1] + 1) / 2, 0.0, 1.0)
]
heading_dir_last = vehicle.last_heading_dir
heading_dir_now = vehicle.heading
# Add more information about the road
if hasattr(current_reference_lane, "heading"):
info.append(clip(current_reference_lane.heading, 0.0, 1.0))
else:
info.append(0.0)
info.append(clip(current_reference_lane.length / DISTANCE, 0.0, 1.0))
if hasattr(current_reference_lane, "direction"):
dir = current_reference_lane.direction
if isinstance(dir, int):
info.append(self._to_zero_and_one(dir))
info.append(0.0)
elif len(dir) == 2:
info.append(self._to_zero_and_one(dir[0]))
info.append(self._to_zero_and_one(dir[1]))
else:
info.extend([0.0, 0.0])
else:
info.extend([0.0, 0.0])
cos_beta = heading_dir_now.dot(heading_dir_last) / (
norm(*heading_dir_now) * norm(*heading_dir_last))
beta_diff = np.arccos(clip(cos_beta, 0.0, 1.0))
yaw_rate = beta_diff / 0.1
info.append(clip(yaw_rate, 0.0, 1.0))
long, lateral = vehicle.lane.local_coordinates(vehicle.position)
info.append(
clip(
(lateral * 2 /
vehicle.routing_localization.get_current_lane_width() + 1.0) /
2.0, 0.0, 1.0))
info.append(clip(long / DISTANCE, 0.0, 1.0))
return info
def heading_diff(self, target_lane):
lateral = None
if isinstance(target_lane, StraightLane):
lateral = np.asarray(
get_vertical_vector(target_lane.end - target_lane.start)[1])
elif isinstance(target_lane, CircularLane):
if target_lane.direction == -1:
lateral = self.position - target_lane.center
else:
lateral = target_lane.center - self.position
else:
raise ValueError("Unknown target lane type: {}".format(
type(target_lane)))
lateral_norm = norm(lateral[0], lateral[1])
forward_direction = self.heading
# print(f"Old forward direction: {self.forward_direction}, new heading {self.heading}")
forward_direction_norm = norm(forward_direction[0],
forward_direction[1])
if not lateral_norm * forward_direction_norm:
return 0
cos = ((forward_direction[0] * lateral[0] +
forward_direction[1] * lateral[1]) /
(lateral_norm * forward_direction_norm))
# return cos
# Normalize to 0, 1
return clip(cos, -1.0, 1.0) / 2 + 0.5
def vehicle_state(self, vehicle):
"""
Wrap vehicle states to list
"""
# update out of road
info = []
if hasattr(vehicle,
"side_detector") and vehicle.side_detector.available:
info += vehicle.side_detector.perceive(
vehicle,
vehicle.engine.physics_world.static_world).cloud_points
else:
lateral_to_left, lateral_to_right, = vehicle.dist_to_left_side, vehicle.dist_to_right_side
total_width = float((vehicle.navigation.map.MAX_LANE_NUM + 1) *
vehicle.navigation.map.MAX_LANE_WIDTH)
lateral_to_left /= total_width
lateral_to_right /= total_width
info += [
clip(lateral_to_left, 0.0, 1.0),
clip(lateral_to_right, 0.0, 1.0)
]
# print("Heading Diff: ", [
# vehicle.heading_diff(current_reference_lane)
# for current_reference_lane in vehicle.navigation.current_ref_lanes
# ])
current_reference_lane = vehicle.navigation.current_ref_lanes[-1]
info += [
vehicle.heading_diff(current_reference_lane),
# Note: speed can be negative denoting free fall. This happen when emergency brake.
clip((vehicle.speed + 1) / (vehicle.max_speed + 1), 0.0, 1.0),
clip((vehicle.steering / vehicle.MAX_STEERING + 1) / 2, 0.0, 1.0),
clip((vehicle.last_current_action[0][0] + 1) / 2, 0.0, 1.0),
clip((vehicle.last_current_action[0][1] + 1) / 2, 0.0, 1.0)
]
heading_dir_last = vehicle.last_heading_dir
heading_dir_now = vehicle.heading
cos_beta = heading_dir_now.dot(heading_dir_last) / (
norm(*heading_dir_now) * norm(*heading_dir_last))
beta_diff = np.arccos(clip(cos_beta, 0.0, 1.0))
# print(beta)
yaw_rate = beta_diff / 0.1
# print(yaw_rate)
info.append(clip(yaw_rate, 0.0, 1.0))
if vehicle.lane_line_detector.available:
info += vehicle.lane_line_detector.perceive(
vehicle,
vehicle.engine.physics_world.static_world).cloud_points
# else:
# _, lateral = vehicle.lane.local_coordinates(vehicle.position)
# info.append(clip((lateral * 2 / vehicle.navigation.map.MAX_LANE_WIDTH + 1.0) / 2.0, 0.0, 1.0))
# add vehicle length/width
if self.config["random_agent_model"]:
info.append(clip(vehicle.LENGTH / vehicle.MAX_LENGTH, 0.0, 1.0))
info.append(clip(vehicle.WIDTH / vehicle.MAX_WIDTH, 0.0, 1.0))
return info
def vehicle_state(vehicle):
"""
Wrap vehicle states to list
"""
# update out of road
info = []
if hasattr(vehicle,
"side_detector") and vehicle.side_detector is not None:
info += vehicle.side_detector.get_cloud_points()
else:
lateral_to_left, lateral_to_right, = vehicle.dist_to_left_side, vehicle.dist_to_right_side
total_width = float(
(vehicle.routing_localization.get_current_lane_num() + 1) *
vehicle.routing_localization.get_current_lane_width())
lateral_to_left /= total_width
lateral_to_right /= total_width
info += [
clip(lateral_to_left, 0.0, 1.0),
clip(lateral_to_right, 0.0, 1.0)
]
# print("Heading Diff: ", [
# vehicle.heading_diff(current_reference_lane)
# for current_reference_lane in vehicle.routing_localization.current_ref_lanes
# ])
current_reference_lane = vehicle.routing_localization.current_ref_lanes[
-1]
info += [
vehicle.heading_diff(current_reference_lane),
# Note: speed can be negative denoting free fall. This happen when emergency brake.
clip((vehicle.speed + 1) / (vehicle.max_speed + 1), 0.0, 1.0),
clip((vehicle.steering / vehicle.max_steering + 1) / 2, 0.0, 1.0),
clip((vehicle.last_current_action[0][0] + 1) / 2, 0.0, 1.0),
clip((vehicle.last_current_action[0][1] + 1) / 2, 0.0, 1.0)
]
heading_dir_last = vehicle.last_heading_dir
heading_dir_now = vehicle.heading
cos_beta = heading_dir_now.dot(heading_dir_last) / (
norm(*heading_dir_now) * norm(*heading_dir_last))
beta_diff = np.arccos(clip(cos_beta, 0.0, 1.0))
# print(beta)
yaw_rate = beta_diff / 0.1
# print(yaw_rate)
info.append(clip(yaw_rate, 0.0, 1.0))
if vehicle.lane_line_detector is not None:
info += vehicle.lane_line_detector.get_cloud_points()
else:
_, lateral = vehicle.lane.local_coordinates(vehicle.position)
info.append(
clip((lateral * 2 /
vehicle.routing_localization.get_current_lane_width() +
1.0) / 2.0, 0.0, 1.0))
return info
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 speed(self):
"""
km/h
"""
velocity = self.chassis_np.node().get_linear_velocity()
speed = norm(velocity[0], velocity[1]) * 3.6
return clip(speed, 0.0, 100000.0)
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 _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 speed(self):
"""
km/h
"""
velocity = self.body.get_linear_velocity()
speed = norm(velocity[0], velocity[1]) * 3.6
return clip(speed, 0.0, 100000.0)
def __init__(self,
start: Vector,
end: Vector,
width: float = AbstractLane.DEFAULT_WIDTH,
line_types: Tuple[LineType, LineType] = (LineType.STRIPED,
LineType.STRIPED),
forbidden: bool = False,
speed_limit: float = 20,
priority: int = 0) -> None:
"""
New straight lane.
:param start: the lane starting position [m]
:param end: the lane ending position [m]
:param width: the lane width [m]
:param line_types: the type of lines on both sides of the lane
:param forbidden: is changing to this lane forbidden
:param priority: priority level of the lane, for determining who has right of way
"""
self.start = np.array(start)
self.end = np.array(end)
self.width = width
self.line_types = line_types or [LineType.STRIPED, LineType.STRIPED]
self.forbidden = forbidden
self.priority = priority
self.speed_limit = speed_limit
self.length = norm((self.end - self.start)[0],
(self.end - self.start)[1])
self.heading = math.atan2(self.end[1] - self.start[1],
self.end[0] - self.start[0])
self.direction = (self.end - self.start) / self.length
self.direction_lateral = np.array(
[-self.direction[1], self.direction[0]])
def update_properties(self):
self.length = norm((self.end - self.start)[0],
(self.end - self.start)[1])
self.heading = math.atan2(self.end[1] - self.start[1],
self.end[0] - self.start[0])
self.direction = (self.end - self.start) / self.length
self.direction_lateral = np.array(
[-self.direction[1], self.direction[0]])
def local_coordinates(self, position: Tuple[float, float]) -> Tuple[float, float]:
delta_x = position[0] - self.center[0]
delta_y = position[1] - self.center[1]
phi = math.atan2(delta_y, delta_x)
phi = self.start_phase + wrap_to_pi(phi - self.start_phase)
r = norm(delta_x, delta_y)
longitudinal = self.direction * (phi - self.start_phase) * self.radius
lateral = self.direction * (self.radius - r)
return longitudinal, lateral
def projection(self, vector):
# Projected to the heading of vehicle
# forward_vector = self.vehicle.get_forward_vector()
# forward_old = (forward_vector[0], -forward_vector[1])
forward = self.heading
# print(f"[projection] Old forward {forward_old}, new heading {forward}")
norm_velocity = norm(forward[0], forward[1]) + 1e-6
project_on_heading = (vector[0] * forward[0] +
vector[1] * forward[1]) / norm_velocity
side_direction = get_vertical_vector(forward)[1]
side_norm = norm(side_direction[0], side_direction[1]) + 1e-6
project_on_side = (vector[0] * side_direction[0] +
vector[1] * side_direction[1]) / side_norm
return project_on_heading, project_on_side
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 cull_distant_traffic_vehicles(cls, vehicles: list, pos,
pg_world: PGWorld):
# Cull distant vehicles
for v in vehicles:
v_p = v.position
if norm(v_p[0] - pos[0],
v_p[1] - v_p[1]) < cls.LOD_VEHICLE_VIS_DIST:
if not v.node_path.hasParent():
v.node_path.reparentTo(pg_world.pbr_worldNP)
else:
if v.node_path.hasParent():
v.node_path.detachNode()
if norm(v_p[0] - pos[0],
v_p[1] - v_p[1]) < cls.LOD_VEHICLE_PHYSICS_DIST:
v.dynamic_nodes.attach_to_physics_world(
pg_world.physics_world.dynamic_world)
else:
v.dynamic_nodes.detach_from_physics_world(
pg_world.physics_world.dynamic_world)
def _update_energy_consumption(self):
"""
The calculation method is from
https://www.researchgate.net/publication/262182035_Reduction_of_Fuel_Consumption_and_Exhaust_Pollutant_Using_Intelligent_Transport_System
default: 3rd gear, try to use ae^bx to fit it, dp: (90, 8), (130, 12)
:return: None
"""
distance = norm(*(self.last_position - self.position)) / 1000 # km
step_energy = 3.25 * math.pow(np.e, 0.01 * self.speed) * distance / 100
# step_energy is in Liter, we return mL
step_energy = step_energy * 1000
self.energy_consumption += step_energy # L/100 km
return step_energy, self.energy_consumption
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 _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 get_surrounding_vehicles_info(self, ego_vehicle, num_others: int = 4):
from pgdrive.utils.math_utils import norm, clip
surrounding_vehicles = list(self.get_surrounding_vehicles())
surrounding_vehicles.sort(
key=lambda v: norm(ego_vehicle.position[0] - v.position[0], ego_vehicle.position[1] - v.position[1])
)
surrounding_vehicles += [None] * num_others
res = []
for vehicle in surrounding_vehicles[:num_others]:
if vehicle is not None:
# assert isinstance(vehicle, IDMVehicle or Base), "Now PGDrive Doesn't support other vehicle type"
relative_position = ego_vehicle.projection(vehicle.position - ego_vehicle.position)
# It is possible that the centroid of other vehicle is too far away from ego but lidar shed on it.
# So the distance may greater than perceive distance.
res.append(clip((relative_position[0] / self.perceive_distance + 1) / 2, 0.0, 1.0))
res.append(clip((relative_position[1] / self.perceive_distance + 1) / 2, 0.0, 1.0))
relative_velocity = ego_vehicle.projection(vehicle.velocity - ego_vehicle.velocity)
res.append(clip((relative_velocity[0] / ego_vehicle.max_speed + 1) / 2, 0.0, 1.0))
res.append(clip((relative_velocity[1] / ego_vehicle.max_speed + 1) / 2, 0.0, 1.0))
else:
res += [0.0] * 4
return res
def overwritten_get_surrounding_vehicles_info(self,
lidar,
ego_vehicle,
num_others: int = 4):
# surrounding_vehicles = list(lidar.get_surrounding_vehicles())
surrounding_vehicles = list(
self._env.scene_manager.traffic_manager.vehicles)[1:]
surrounding_vehicles.sort(
key=lambda v: norm(ego_vehicle.position[0] - v.position[0],
ego_vehicle.position[1] - v.position[1]))
# dis = [(str(v), norm(ego_vehicle.position[0] - v.position[0], ego_vehicle.position[1] - v.position[1]))
# for v in surrounding_vehicles]
surrounding_vehicles += [None] * num_others
res = []
for vehicle in surrounding_vehicles[:num_others]:
if vehicle is not None:
relative_position = ego_vehicle.projection(
vehicle.position - ego_vehicle.position)
dist = norm(relative_position[0], relative_position[1])
if dist > DISTANCE:
if self.config["use_extra_state"]:
res += [0.0] * self._traffic_vehicle_state_dim
else:
res += [0.0] * self._traffic_vehicle_state_dim_wo_extra
continue
relative_velocity = ego_vehicle.projection(
vehicle.velocity - ego_vehicle.velocity)
res.append(clip(dist / DISTANCE, 0.0, 1.0))
res.append(
clip(
norm(relative_velocity[0], relative_velocity[1]) /
ego_vehicle.max_speed, 0.0, 1.0))
res.append(
clip((relative_position[0] / DISTANCE + 1) / 2, 0.0, 1.0))
res.append(
clip((relative_position[1] / DISTANCE + 1) / 2, 0.0, 1.0))
res.append(
clip(
(relative_velocity[0] / ego_vehicle.max_speed + 1) / 2,
0.0, 1.0))
res.append(
clip(
(relative_velocity[1] / ego_vehicle.max_speed + 1) / 2,
0.0, 1.0))
if self.config["use_extra_state"]:
res.extend(self.traffic_vehicle_state(vehicle))
else:
if self.config["use_extra_state"]:
res += [0.0] * self._traffic_vehicle_state_dim
else:
res += [0.0] * self._traffic_vehicle_state_dim_wo_extra
# p1 = []
# p2 = []
# for vehicle in surrounding_vehicles[:num_others]:
# if vehicle is not None:
# relative_position = ego_vehicle.projection(vehicle.position - ego_vehicle.position)
# relative_velocity = ego_vehicle.projection(vehicle.velocity - ego_vehicle.velocity)
# p1.append(relative_position)
# p2.append(relative_velocity)
# print("Detected Others Position: {}, Velocity: {}".format(p1, p2))
return res
def update_navigation_localization(self, ego_vehicle):
position = ego_vehicle.position
lane, lane_index = ray_localization(position, ego_vehicle.pg_world)
if lane is None:
lane, lane_index = ego_vehicle.lane, ego_vehicle.lane_index
if self.FORCE_CALCULATE:
lane_index, _ = self.map.road_network.get_closest_lane_index(
position)
lane = self.map.road_network.get_lane(lane_index)
self._update_target_checkpoints(lane_index)
target_road_1_start = self.checkpoints[
self.target_checkpoints_index[0]]
target_road_1_end = self.checkpoints[self.target_checkpoints_index[0] +
1]
target_road_2_start = self.checkpoints[
self.target_checkpoints_index[1]]
target_road_2_end = self.checkpoints[self.target_checkpoints_index[1] +
1]
target_lanes_1 = self.map.road_network.graph[target_road_1_start][
target_road_1_end]
target_lanes_2 = self.map.road_network.graph[target_road_2_start][
target_road_2_end]
res = []
self.current_ref_lanes = target_lanes_1
ckpts = []
lanes_heading = []
for lanes_id, lanes in enumerate([target_lanes_1, target_lanes_2]):
ref_lane = lanes[0]
later_middle = (float(self.map.lane_num) / 2 -
0.5) * self.map.lane_width
if isinstance(ref_lane, CircularLane) and ref_lane.direction == 1:
ref_lane = lanes[-1]
later_middle *= -1
check_point = ref_lane.position(ref_lane.length, later_middle)
if lanes_id == 0:
# calculate ego v lane heading
lanes_heading.append(
ref_lane.heading_at(
ref_lane.local_coordinates(ego_vehicle.position)[0]))
else:
lanes_heading.append(
ref_lane.heading_at(
min(self.PRE_NOTIFY_DIST, ref_lane.length)))
ckpts.append(check_point)
dir_vec = check_point - ego_vehicle.position
dir_norm = norm(dir_vec[0], dir_vec[1])
if dir_norm > self.NAVI_POINT_DIST:
dir_vec = dir_vec / dir_norm * self.NAVI_POINT_DIST
proj_heading, proj_side = ego_vehicle.projection(dir_vec)
bendradius = 0.0
dir = 0.0
angle = 0.0
if isinstance(ref_lane, CircularLane):
bendradius = ref_lane.radius / (
BlockParameterSpace.CURVE[Parameter.radius].max +
self.map.lane_num * self.map.lane_width)
dir = ref_lane.direction
if dir == 1:
angle = ref_lane.end_phase - ref_lane.start_phase
elif dir == -1:
angle = ref_lane.start_phase - ref_lane.end_phase
res += [
clip((proj_heading / self.NAVI_POINT_DIST + 1) / 2, 0.0, 1.0),
clip((proj_side / self.NAVI_POINT_DIST + 1) / 2, 0.0, 1.0),
clip(bendradius, 0.0, 1.0),
clip((dir + 1) / 2, 0.0, 1.0),
clip((np.rad2deg(angle) /
BlockParameterSpace.CURVE[Parameter.angle].max + 1) / 2,
0.0, 1.0)
]
if self.show_navi_point:
pos_of_goal = ckpts[0]
self.goal_node_path.setPos(pos_of_goal[0], -pos_of_goal[1], 1.8)
self.goal_node_path.setH(self.goal_node_path.getH() + 3)
self.update_navi_arrow(lanes_heading)
self.navi_info = res
return lane, lane_index
def _add_lane(self, lane: AbstractLane, lane_id: int, colors: List[Vec4]):
parent_np = self.lane_line_node_path
lane_width = lane.width_at(0)
for k, i in enumerate([-1, 1]):
line_color = colors[k]
if lane.line_types[k] == LineType.NONE or (lane_id != 0
and k == 0):
if isinstance(lane, StraightLane):
continue
elif isinstance(
lane, CircularLane) and lane.radius != lane_width / 2:
# for ramp render
continue
if lane.line_types[k] == LineType.CONTINUOUS or lane.line_types[
k] == LineType.SIDE:
if isinstance(lane, StraightLane):
lane_start = lane.position(0, i * lane_width / 2)
lane_end = lane.position(lane.length, i * lane_width / 2)
middle = lane.position(lane.length / 2, i * lane_width / 2)
self._add_lane_line2bullet(lane_start, lane_end, middle,
parent_np, line_color,
lane.line_types[k])
elif isinstance(lane, CircularLane):
segment_num = int(lane.length /
Block.CIRCULAR_SEGMENT_LENGTH)
for segment in range(segment_num):
lane_start = lane.position(
segment * Block.CIRCULAR_SEGMENT_LENGTH,
i * lane_width / 2)
lane_end = lane.position(
(segment + 1) * Block.CIRCULAR_SEGMENT_LENGTH,
i * lane_width / 2)
middle = (lane_start + lane_end) / 2
self._add_lane_line2bullet(lane_start, lane_end,
middle, parent_np,
line_color,
lane.line_types[k])
# for last part
lane_start = lane.position(
segment_num * Block.CIRCULAR_SEGMENT_LENGTH,
i * lane_width / 2)
lane_end = lane.position(lane.length, i * lane_width / 2)
middle = (lane_start + lane_end) / 2
self._add_lane_line2bullet(lane_start, lane_end, middle,
parent_np, line_color,
lane.line_types[k])
if lane.line_types[k] == LineType.SIDE:
radius = lane.radius if isinstance(lane,
CircularLane) else 0.0
segment_num = int(lane.length / Block.SIDEWALK_LENGTH)
for segment in range(segment_num):
lane_start = lane.position(
segment * Block.SIDEWALK_LENGTH,
i * lane_width / 2)
lane_end = lane.position(
(segment + 1) * Block.SIDEWALK_LENGTH,
i * lane_width / 2)
middle = (lane_start + lane_end) / 2
self._add_sidewalk2bullet(lane_start, lane_end, middle,
radius, lane.direction)
# for last part
lane_start = lane.position(
segment_num * Block.SIDEWALK_LENGTH,
i * lane_width / 2)
lane_end = lane.position(lane.length, i * lane_width / 2)
middle = (lane_start + lane_end) / 2
if norm(lane_start[0] - lane_end[0],
lane_start[1] - lane_end[1]) > 1e-1:
self._add_sidewalk2bullet(lane_start, lane_end, middle,
radius, lane.direction)
elif lane.line_types[k] == LineType.BROKEN:
straight = True if isinstance(lane, StraightLane) else False
segment_num = int(lane.length / (2 * Block.STRIPE_LENGTH))
for segment in range(segment_num):
lane_start = lane.position(
segment * Block.STRIPE_LENGTH * 2, i * lane_width / 2)
lane_end = lane.position(
segment * Block.STRIPE_LENGTH * 2 +
Block.STRIPE_LENGTH, i * lane_width / 2)
middle = lane.position(
segment * Block.STRIPE_LENGTH * 2 +
Block.STRIPE_LENGTH / 2, i * lane_width / 2)
self._add_lane_line2bullet(lane_start, lane_end, middle,
parent_np, line_color,
lane.line_types[k], straight)
lane_start = lane.position(
segment_num * Block.STRIPE_LENGTH * 2, i * lane_width / 2)
lane_end = lane.position(lane.length + Block.STRIPE_LENGTH,
i * lane_width / 2)
middle = (lane_end[0] + lane_start[0]) / 2, (lane_end[1] +
lane_start[1]) / 2
if not straight:
self._add_lane_line2bullet(lane_start, lane_end, middle,
parent_np, line_color,
lane.line_types[k], straight)
if straight:
lane_start = lane.position(0, i * lane_width / 2)
lane_end = lane.position(lane.length, i * lane_width / 2)
middle = lane.position(lane.length / 2, i * lane_width / 2)
self._add_box_body(lane_start, lane_end, middle, parent_np,
lane.line_types[k], line_color)
def all_distance_greater_than(distance, poses, target_pos):
v_p = target_pos
for pos in poses:
if norm(v_p[0] - pos[0], v_p[1] - v_p[1]) < distance:
return False
return True
def _add_pgdrive_lanes(self, lane, lane_id, lane_width, colors, parent_np):
# for pgdrive structure
for k, i in enumerate([-1, 1]):
line_color = colors[k]
if lane.line_types[k] == LineType.NONE or (lane_id != 0
and k == 0):
if isinstance(lane, StraightLane):
continue
elif isinstance(
lane, CircularLane) and lane.radius != lane_width / 2:
# for ramp render
continue
if lane.line_types[k] == LineType.CONTINUOUS or lane.line_types[
k] == LineType.SIDE:
if isinstance(lane, StraightLane):
lane_start = lane.position(0, i * lane_width / 2)
lane_end = lane.position(lane.length, i * lane_width / 2)
middle = lane.position(lane.length / 2, i * lane_width / 2)
self._add_lane_line2bullet(lane_start, lane_end, middle,
parent_np, line_color,
lane.line_types[k])
elif isinstance(lane, CircularLane):
segment_num = int(
lane.length /
DrivableAreaProperty.CIRCULAR_SEGMENT_LENGTH)
for segment in range(segment_num):
lane_start = lane.position(
segment *
DrivableAreaProperty.CIRCULAR_SEGMENT_LENGTH,
i * lane_width / 2)
lane_end = lane.position(
(segment + 1) *
DrivableAreaProperty.CIRCULAR_SEGMENT_LENGTH,
i * lane_width / 2)
middle = (lane_start + lane_end) / 2
self._add_lane_line2bullet(lane_start, lane_end,
middle, parent_np,
line_color,
lane.line_types[k])
# for last part
lane_start = lane.position(
segment_num *
DrivableAreaProperty.CIRCULAR_SEGMENT_LENGTH,
i * lane_width / 2)
lane_end = lane.position(lane.length, i * lane_width / 2)
middle = (lane_start + lane_end) / 2
self._add_lane_line2bullet(lane_start, lane_end, middle,
parent_np, line_color,
lane.line_types[k])
if lane.line_types[k] == LineType.SIDE:
radius = lane.radius if isinstance(lane,
CircularLane) else 0.0
segment_num = int(lane.length /
DrivableAreaProperty.SIDEWALK_LENGTH)
for segment in range(segment_num):
lane_start = lane.position(
segment * DrivableAreaProperty.SIDEWALK_LENGTH,
i * lane_width / 2)
lane_end = lane.position(
(segment + 1) *
DrivableAreaProperty.SIDEWALK_LENGTH,
i * lane_width / 2)
middle = (lane_start + lane_end) / 2
self._add_sidewalk2bullet(lane_start, lane_end, middle,
radius, lane.direction)
# for last part
lane_start = lane.position(
segment_num * DrivableAreaProperty.SIDEWALK_LENGTH,
i * lane_width / 2)
lane_end = lane.position(lane.length, i * lane_width / 2)
middle = (lane_start + lane_end) / 2
if norm(lane_start[0] - lane_end[0],
lane_start[1] - lane_end[1]) > 1e-1:
self._add_sidewalk2bullet(lane_start, lane_end, middle,
radius, lane.direction)
elif lane.line_types[k] == LineType.BROKEN:
straight = True if isinstance(lane, StraightLane) else False
segment_num = int(lane.length /
(2 * DrivableAreaProperty.STRIPE_LENGTH))
for segment in range(segment_num):
lane_start = lane.position(
segment * DrivableAreaProperty.STRIPE_LENGTH * 2,
i * lane_width / 2)
lane_end = lane.position(
segment * DrivableAreaProperty.STRIPE_LENGTH * 2 +
DrivableAreaProperty.STRIPE_LENGTH, i * lane_width / 2)
middle = lane.position(
segment * DrivableAreaProperty.STRIPE_LENGTH * 2 +
DrivableAreaProperty.STRIPE_LENGTH / 2,
i * lane_width / 2)
self._add_lane_line2bullet(lane_start, lane_end, middle,
parent_np, line_color,
lane.line_types[k], straight)
lane_start = lane.position(
segment_num * DrivableAreaProperty.STRIPE_LENGTH * 2,
i * lane_width / 2)
lane_end = lane.position(
lane.length + DrivableAreaProperty.STRIPE_LENGTH,
i * lane_width / 2)
middle = (lane_end[0] + lane_start[0]) / 2, (lane_end[1] +
lane_start[1]) / 2
if not straight:
self._add_lane_line2bullet(lane_start, lane_end, middle,
parent_np, line_color,
lane.line_types[k], straight)
if straight:
lane_start = lane.position(0, i * lane_width / 2)
lane_end = lane.position(lane.length, i * lane_width / 2)
middle = lane.position(lane.length / 2, i * lane_width / 2)
self._add_box_body(lane_start, lane_end, middle, parent_np,
lane.line_types[k], line_color)
