def _extrapolate_to_now(vs: VehicleState, staleness: float,
lin_acc_slope: float, ang_acc_slope: float):
"""Here we just linearly extrapolate the acceleration to "now" from the previous state for
each vehicle and then use standard kinematics to project the velocity and position from that."""
# The following ~10 lines are a hack b/c I'm too stupid to figure out
# how to do calculus on quaternions...
heading = vs.pose.heading
heading_delta_vec = staleness * (
vs.angular_velocity + 0.5 * vs.angular_acceleration * staleness +
ang_acc_slope * staleness**2 / 6.0)
heading += vec_to_radians(heading_delta_vec[:2]) + (0.5 * math.pi)
heading %= 2 * math.pi
vs.pose.orientation = fast_quaternion_from_angle(heading)
# XXX: also need to remove the cached heading_ since we've changed orientation
vs.pose.heading_ = None
# I assume the following should be updated based on changing
# heading from above, but I'll leave that for now...
vs.pose.position += staleness * (
vs.linear_velocity + 0.5 * vs.linear_acceleration * staleness +
lin_acc_slope * staleness**2 / 6.0)
vs.linear_velocity += staleness * (vs.linear_acceleration +
0.5 * lin_acc_slope * staleness)
vs.speed = np.linalg.norm(vs.linear_velocity)
vs.angular_velocity += staleness * (vs.angular_acceleration +
0.5 * ang_acc_slope * staleness)
vs.linear_acceleration += staleness * lin_acc_slope
vs.angular_acceleration += staleness * ang_acc_slope
def scan_for_vehicles(
target_prefix,
angle_a,
angle_b,
activation_dist_squared,
self_vehicle_state,
other_vehicle_states,
short_circuit: bool = False,
):
if target_prefix:
other_vehicle_states = filter(
lambda v: v.id.startswith(target_prefix), other_vehicle_states
)
min_angle, max_angle = min(angle_a, angle_b), max(angle_a, angle_b)
vehicles = []
for vehicle_state in other_vehicle_states:
sqd = squared_dist(self_vehicle_state.position, vehicle_state.position)
# check for activation distance
if sqd < activation_dist_squared:
direction = np.sum(
[vehicle_state.position, -self_vehicle_state.position], axis=0
)
angle = Heading(vec_to_radians(direction[:2]))
rel_angle = angle.relative_to(self_vehicle_state.heading)
if min_angle <= rel_angle <= max_angle:
vehicles.append(vehicle_state)
if short_circuit:
break
return vehicles
def to_position_and_heading(road_id, lane_index, offset, road_map):
road = road_map.road_by_id(road_id)
lane = road.lane_at_index(lane_index)
offset = resolve_offset(offset, lane.length)
position = lane.from_lane_coord(RefLinePoint(s=offset))
lane_vector = lane.vector_at_offset(offset)
heading = vec_to_radians(lane_vector[:2])
return position, Heading(heading)
def to_position_and_heading(edge_id, lane_index, offset, road_network):
edge = road_network.edge_by_id(edge_id)
lane = edge.getLanes()[lane_index]
offset = resolve_offset(offset, lane.getLength())
position = road_network.world_coord_from_offset(lane, offset)
lane_vector = road_network.lane_vector_at_offset(lane, offset)
heading = vec_to_radians(lane_vector)
return tuple(position), Heading(heading)
def scan_for_vehicles(
target_prefix,
angle_a,
angle_b,
activation_dist_squared,
self_vehicle_state,
other_vehicle_states,
short_circuit: bool = False,
):
"""Sense test for vehicles within a semi-circle radius of a vehicle.
Args:
target_prefix:
The whitelist of vehicles with vehicle_ids starting with this prefix for quick elimination.
angle_a:
The minimum sweep angle between -pi and pi.
angle_b:
The maximum sweep angle between -pi and pi.
activation_dist_squared:
The distance to check for the target.
self_vehicle_state:
The vehicle state of the vehicle that is scanning.
other_vehicle_states:
The set of vehicles to test for.
Returns:
If the tested for vehicle is within the semi-circle range of the base vehicle.
"""
if target_prefix:
other_vehicle_states = filter(lambda v: v.id.startswith(target_prefix),
other_vehicle_states)
min_angle, max_angle = min(angle_a, angle_b), max(angle_a, angle_b)
vehicles = []
for vehicle_state in other_vehicle_states:
sqd = squared_dist(self_vehicle_state.position, vehicle_state.position)
# check for activation distance
if sqd < activation_dist_squared:
direction = np.sum(
[vehicle_state.position, -self_vehicle_state.position], axis=0)
angle = Heading(vec_to_radians(direction[:2]))
rel_angle = angle.relative_to(self_vehicle_state.heading)
if min_angle <= rel_angle <= max_angle:
vehicles.append(vehicle_state)
if short_circuit:
break
return vehicles
def _cal_heading(self, window_param) -> float:
window = window_param[0]
new_heading = 0
prev_heading = None
den = 0
for w in range(self._heading_window - 1):
c = window[w, :2]
n = window[w + 1, :2]
if any(np.isnan(c)) or any(np.isnan(n)):
if prev_heading is not None:
new_heading += prev_heading
den += 1
continue
s = np.linalg.norm(n - c)
ispeed = window[w, 2]
if s == 0.0 or (
self._heading_min_speed is not None
and (s < self._heading_min_speed or ispeed < self._heading_min_speed)
):
if prev_heading is not None:
new_heading += prev_heading
den += 1
continue
vhat = (n - c) / s
inst_heading = vec_to_radians(vhat)
if prev_heading is not None:
if inst_heading == 0 and prev_heading > math.pi:
inst_heading = 2 * math.pi
if self._max_angular_velocity:
# XXX: could try to divide by sim_time delta here instead of assuming .1s
angular_velocity = (inst_heading - prev_heading) / 0.1
if abs(angular_velocity) > self._max_angular_velocity:
inst_heading = (
prev_heading
+ np.sign(angular_velocity)
* self._max_angular_velocity
* 0.1
)
den += 1
new_heading += inst_heading
prev_heading = inst_heading
if den > 0:
new_heading /= den
else:
new_heading = self._prev_heading
self._prev_heading = new_heading
return new_heading % (2 * math.pi)
def scan_for_vehicle(
target_prefix: str,
angle_a: float,
angle_b: float,
activation_dist_squared: float,
self_vehicle_state,
other_vehicle_state,
):
"""Sense test for another vehicle within a semi-circle range of a vehicle.
Args:
target_prefix:
The whitelist of vehicles with vehicle_ids starting with this prefix for quick elimination.
angle_a:
The minimum sweep angle between -pi and pi.
angle_b:
The maximum sweep angle between -pi and pi.
activation_dist_squared:
The distance to check for the target.
self_vehicle_state:
The vehicle state of the vehicle that is scanning.
other_vehicle_state:
The vehicle to test for.
Returns:
If the tested for vehicle is within the semi-circle range of the base vehicle.
"""
if target_prefix and not other_vehicle_state.id.startswith(target_prefix):
return False
min_angle, max_angle = min(angle_a, angle_b), max(angle_a, angle_b)
sqd = squared_dist(self_vehicle_state.position,
other_vehicle_state.position)
# check for activation distance
if sqd < activation_dist_squared:
direction = np.sum(
[other_vehicle_state.position, -self_vehicle_state.position],
axis=0)
angle = Heading(vec_to_radians(direction[:2]))
rel_angle = angle.relative_to(self_vehicle_state.heading)
return min_angle <= rel_angle <= max_angle
return False
def column_val_in_row(self, row, col_name: str) -> Any:
row_name = self._col_map.get(col_name)
if row_name:
return row[row_name]
if col_name == "length":
return row.get("length", 0.0)
if col_name == "width":
return row.get("width", 0.0)
if col_name == "type":
return self._lookup_agent_type(row["agent_type"])
if col_name == "speed":
if self._next_row:
# XXX: could try to divide by sim_time delta here instead of assuming .1s
dx = (float(self._next_row["x"]) - float(row["x"])) / 0.1
dy = (float(self._next_row["y"]) - float(row["y"])) / 0.1
else:
dx, dy = float(row["vx"]), float(row["vy"])
return np.linalg.norm((dx, dy))
if col_name == "heading_rad":
if self._next_row:
dx = float(self._next_row["x"]) - float(row["x"])
dy = float(self._next_row["y"]) - float(row["y"])
dm = np.linalg.norm((dx, dy))
if dm != 0.0 and dm > self._heading_min_speed:
new_heading = vec_to_radians((dx, dy))
if self._max_angular_velocity and self._prev_heading is not None:
# XXX: could try to divide by sim_time delta here instead of assuming .1s
angular_velocity = (new_heading - self._prev_heading) / 0.1
if abs(angular_velocity) > self._max_angular_velocity:
new_heading = (
self._prev_heading
+ np.sign(angular_velocity)
* self._max_angular_velocity
* 0.1
)
self._prev_heading = new_heading
return new_heading
# Note: pedestrian track files won't have this
self._prev_heading = float(row.get("psi_rad", 0.0)) - math.pi / 2
return self._prev_heading
# XXX: should probably check for and handle x_offset_px here too like in NGSIM
return None
def scan_for_vehicle(
target_prefix,
angle_a,
angle_b,
activation_dist_squared,
self_vehicle_state,
other_vehicle_state,
):
if target_prefix and not other_vehicle_state.id.startswith(target_prefix):
return False
min_angle, max_angle = min(angle_a, angle_b), max(angle_a, angle_b)
sqd = squared_dist(self_vehicle_state.position, other_vehicle_state.position)
# check for activation distance
if sqd < activation_dist_squared:
direction = np.sum(
[other_vehicle_state.position, -self_vehicle_state.position], axis=0
)
angle = Heading(vec_to_radians(direction[:2]))
rel_angle = angle.relative_to(self_vehicle_state.heading)
return min_angle <= rel_angle <= max_angle
return False
def yaw_rate(self) -> float:
"""Returns 2-D rotational speed in rad/sec."""
_, velocity_rotational = np.array(
self._client.getBaseVelocity(self._bullet_id))
return vec_to_radians(velocity_rotational[:2])
def _shape_lanepoints_along_lane(
self, lane: Lane, lanepoint_by_lane_memo: dict
) -> Tuple[LinkedLanePoint, List[LinkedLanePoint]]:
lane_queue = queue.Queue()
lane_queue.put((lane, None))
shape_lanepoints = []
initial_lanepoint = None
while not lane_queue.empty():
lane, previous_lp = lane_queue.get()
first_lanepoint = lanepoint_by_lane_memo.get(lane.getID())
if first_lanepoint:
if previous_lp:
previous_lp.nexts.append(first_lanepoint)
continue
lane_data = self._road_network.lane_data_for_lane(lane)
lane_shape = [np.array(p) for p in lane.getShape(False)]
assert len(lane_shape) >= 2, repr(lane_shape)
heading = vec_to_radians(lane_shape[1] - lane_shape[0])
heading = Heading(heading)
first_lanepoint = LinkedLanePoint(
lp=LanePoint(
pos=lane_shape[0],
heading=heading,
lane_width=lane.getWidth(),
speed_limit=lane_data.lane_speed,
lane_id=lane.getID(),
lane_index=lane.getIndex(),
),
nexts=[],
is_shape_lp=True,
)
if previous_lp is not None:
previous_lp.nexts.append(first_lanepoint)
if initial_lanepoint is None:
initial_lanepoint = first_lanepoint
lanepoint_by_lane_memo[lane.getID()] = first_lanepoint
shape_lanepoints.append(first_lanepoint)
curr_lanepoint = first_lanepoint
for p1, p2 in zip(lane_shape[1:], lane_shape[2:]):
heading_ = vec_to_radians(p2 - p1)
heading_ = Heading(heading_)
linked_lanepoint = LinkedLanePoint(
lp=LanePoint(
pos=p1,
heading=heading_,
lane_width=lane.getWidth(),
speed_limit=lane_data.lane_speed,
lane_id=lane.getID(),
lane_index=lane.getIndex(),
),
nexts=[],
is_shape_lp=True,
)
shape_lanepoints.append(linked_lanepoint)
curr_lanepoint.nexts.append(linked_lanepoint)
curr_lanepoint = linked_lanepoint
# Add a lanepoint for the last point of the current lane
last_linked_lanepoint = LinkedLanePoint(
lp=LanePoint(
pos=lane_shape[-1],
heading=curr_lanepoint.lp.heading,
lane_width=curr_lanepoint.lp.lane_width,
speed_limit=curr_lanepoint.lp.speed_limit,
lane_id=curr_lanepoint.lp.lane_id,
lane_index=curr_lanepoint.lp.lane_index,
),
nexts=[],
is_shape_lp=True,
)
shape_lanepoints.append(last_linked_lanepoint)
curr_lanepoint.nexts.append(last_linked_lanepoint)
curr_lanepoint = last_linked_lanepoint
for out_connection in lane.getOutgoing():
out_lane = out_connection.getToLane()
# Use internal lanes of junctions (if we're at a junction)
via_lane_id = out_connection.getViaLaneID()
if via_lane_id:
out_lane = self._road_network.lane_by_id(via_lane_id)
lane_queue.put((out_lane, curr_lanepoint))
return initial_lanepoint, shape_lanepoints
def _process_interp_for_lane_lp(
shape_lp: LinkedLanePoint,
first_linked_lanepoint: LinkedLanePoint,
next_shape_lp: LinkedLanePoint,
spacing: float,
newly_created_lanepoints: List[LinkedLanePoint],
) -> LinkedLanePoint:
lane_id = shape_lp.lp.lane_id
curr_lanepoint = first_linked_lanepoint
lane_seg_vec = next_shape_lp.lp.pos - shape_lp.lp.pos
lane_seg_len = np.linalg.norm(lane_seg_vec)
# We set the initial distance into the lane at `spacing` because
# we already have a lanepoint along this segment (curr_lanepoint)
dist_into_lane_seg = spacing
while dist_into_lane_seg < lane_seg_len:
p = dist_into_lane_seg / lane_seg_len
pos = shape_lp.lp.pos + lane_seg_vec * p
# The thresholds for calculating last lanepoint. If the
# midpoint between the current lanepoint and the next shape
# lanepoint is less than the minimum distance then the last
# lanepoint position will be that midpoint. If the midpoint
# is closer than last spacing threshold to the next shape
# lanepoint, then the last lanepoint will be the current
# lanepoint.
# XXX: the map scale should be taken into account here.
last_spacing_threshold_dist = 0.8 * spacing
minimum_dist_next_shape_lp = 1.4
half_distant_current_next_shape_lp = np.linalg.norm(
0.5 * (curr_lanepoint.lp.pos - next_shape_lp.lp.pos))
mid_point_current_next_shape_lp = 0.5 * (next_shape_lp.lp.pos +
curr_lanepoint.lp.pos)
if half_distant_current_next_shape_lp < minimum_dist_next_shape_lp:
pos = mid_point_current_next_shape_lp
dist_pos_next_shape_lp = np.linalg.norm(next_shape_lp.lp.pos - pos)
if dist_pos_next_shape_lp < last_spacing_threshold_dist:
break
heading = vec_to_radians(lane_seg_vec)
lane_width = lerp(shape_lp.lp.lane_width,
next_shape_lp.lp.lane_width, p)
speed_limit = lerp(shape_lp.lp.speed_limit,
next_shape_lp.lp.speed_limit, p)
linked_lanepoint = LinkedLanePoint(
lp=LanePoint(
pos=pos,
heading=Heading(heading),
lane_width=lane_width,
speed_limit=speed_limit,
lane_id=lane_id,
lane_index=shape_lp.lp.lane_index,
),
nexts=[],
is_shape_lp=False,
)
curr_lanepoint.nexts.append(linked_lanepoint)
curr_lanepoint = linked_lanepoint
newly_created_lanepoints.append(linked_lanepoint)
dist_into_lane_seg += spacing
return curr_lanepoint
def _process_interp_for_lane_lp(
shape_lp: LinkedLanePoint,
first_linked_lanepoint: LinkedLanePoint,
next_shape_lp: LinkedLanePoint,
spacing: float,
newly_created_lanepoints: List[LinkedLanePoint],
) -> LinkedLanePoint:
rmlane = shape_lp.lp.lane
curr_lanepoint = first_linked_lanepoint
lane_seg_vec = next_shape_lp.lp.pose.position - shape_lp.lp.pose.position
lane_seg_len = np.linalg.norm(lane_seg_vec)
# We set the initial distance into the lane at `spacing` because
# we already have a lanepoint along this segment (curr_lanepoint)
dist_into_lane_seg = spacing
while dist_into_lane_seg < lane_seg_len:
p = dist_into_lane_seg / lane_seg_len
pos = shape_lp.lp.pose.position + lane_seg_vec * p
# The thresholds for calculating last lanepoint. If the
# midpoint between the current lanepoint and the next shape
# lanepoint is less than the minimum distance then the last
# lanepoint position will be that midpoint. If the midpoint
# is closer than last spacing threshold to the next shape
# lanepoint, then the last lanepoint will be the current
# lanepoint.
# XXX: the map scale should be taken into account here.
last_spacing_threshold_dist = 0.8 * spacing
minimum_dist_next_shape_lp = 1.4
half_distant_current_next_shape_lp = np.linalg.norm(
0.5 * (curr_lanepoint.lp.pose.position -
next_shape_lp.lp.pose.position))
mid_point_current_next_shape_lp = 0.5 * (
next_shape_lp.lp.pose.position +
curr_lanepoint.lp.pose.position)
if half_distant_current_next_shape_lp < minimum_dist_next_shape_lp:
pos = mid_point_current_next_shape_lp
dist_pos_next_shape_lp = np.linalg.norm(
next_shape_lp.lp.pose.position - pos)
if dist_pos_next_shape_lp < last_spacing_threshold_dist:
break
heading = vec_to_radians(lane_seg_vec)
orientation = fast_quaternion_from_angle(heading)
rmlane_coord = rmlane.to_lane_coord(
Point(x=pos[0], y=pos[1], z=0.0))
linked_lanepoint = LinkedLanePoint(
lp=LanePoint(
lane=rmlane,
pose=Pose(position=pos, orientation=orientation),
lane_width=rmlane.width_at_offset(rmlane_coord.s),
),
nexts=[],
is_inferred=True,
)
curr_lanepoint.nexts.append(linked_lanepoint)
curr_lanepoint = linked_lanepoint
newly_created_lanepoints.append(linked_lanepoint)
dist_into_lane_seg += spacing
return curr_lanepoint
def _shape_lanepoints_along_lane(
road_map: OpenDriveRoadNetwork,
lane: RoadMap.Lane,
lanepoint_by_lane_memo: dict,
) -> Tuple[LinkedLanePoint, List[LinkedLanePoint]]:
lane_queue = queue.Queue()
lane_queue.put((lane, None))
shape_lanepoints = []
initial_lanepoint = None
while not lane_queue.empty():
curr_lane, previous_lp = lane_queue.get()
first_lanepoint = lanepoint_by_lane_memo.get(curr_lane.lane_id)
if first_lanepoint:
if previous_lp:
previous_lp.nexts.append(first_lanepoint)
continue
lane_shape = [np.array(p) for p in curr_lane.centerline_points]
assert len(lane_shape) >= 2, repr(lane_shape)
heading = vec_to_radians(lane_shape[1] - lane_shape[0])
heading = Heading(heading)
orientation = fast_quaternion_from_angle(heading)
first_lane_coord = curr_lane.to_lane_coord(
Point(x=lane_shape[0][0], y=lane_shape[0][1], z=0.0))
first_lanepoint = LinkedLanePoint(
lp=LanePoint(
lane=curr_lane,
pose=Pose(position=lane_shape[0],
orientation=orientation),
lane_width=curr_lane.width_at_offset(
first_lane_coord.s),
),
nexts=[],
is_inferred=False,
)
if previous_lp is not None:
previous_lp.nexts.append(first_lanepoint)
if initial_lanepoint is None:
initial_lanepoint = first_lanepoint
lanepoint_by_lane_memo[curr_lane.lane_id] = first_lanepoint
shape_lanepoints.append(first_lanepoint)
curr_lanepoint = first_lanepoint
for p1, p2 in zip(lane_shape[1:], lane_shape[2:]):
heading_ = vec_to_radians(p2 - p1)
heading_ = Heading(heading_)
orientation_ = fast_quaternion_from_angle(heading_)
lp_lane_coord = curr_lane.to_lane_coord(
Point(x=p1[0], y=p1[1], z=0.0))
linked_lanepoint = LinkedLanePoint(
lp=LanePoint(
lane=curr_lane,
pose=Pose(position=p1, orientation=orientation_),
lane_width=curr_lane.width_at_offset(
lp_lane_coord.s),
),
nexts=[],
is_inferred=False,
)
shape_lanepoints.append(linked_lanepoint)
curr_lanepoint.nexts.append(linked_lanepoint)
curr_lanepoint = linked_lanepoint
# Add a lanepoint for the last point of the current lane
last_lane_coord = curr_lanepoint.lp.lane.to_lane_coord(
Point(x=lane_shape[-1][0], y=lane_shape[-1][1], z=0.0))
last_linked_lanepoint = LinkedLanePoint(
lp=LanePoint(
lane=curr_lanepoint.lp.lane,
pose=Pose(
position=lane_shape[-1],
orientation=curr_lanepoint.lp.pose.orientation,
),
lane_width=curr_lanepoint.lp.lane.width_at_offset(
last_lane_coord.s),
),
nexts=[],
is_inferred=False,
)
shape_lanepoints.append(last_linked_lanepoint)
curr_lanepoint.nexts.append(last_linked_lanepoint)
curr_lanepoint = last_linked_lanepoint
outgoing_roads_added = []
for out_lane in curr_lane.outgoing_lanes:
if out_lane.is_drivable:
lane_queue.put((out_lane, curr_lanepoint))
outgoing_road = out_lane.road
if out_lane.road not in outgoing_roads_added:
outgoing_roads_added.append(outgoing_road)
for outgoing_road in outgoing_roads_added:
for next_lane in outgoing_road.lanes:
if (next_lane.is_drivable
and next_lane not in curr_lane.outgoing_lanes):
lane_queue.put((next_lane, curr_lanepoint))
return initial_lanepoint, shape_lanepoints
def _shape_lanepoints_along_lane(
road_map: SumoRoadNetwork, lane: RoadMap.Lane,
lanepoint_by_lane_memo: dict
) -> Tuple[LinkedLanePoint, List[LinkedLanePoint]]:
lane_queue = queue.Queue()
lane_queue.put((lane, None))
shape_lanepoints = []
initial_lanepoint = None
while not lane_queue.empty():
lane, previous_lp = lane_queue.get()
first_lanepoint = lanepoint_by_lane_memo.get(lane.getID())
if first_lanepoint:
if previous_lp:
previous_lp.nexts.append(first_lanepoint)
continue
lane_shape = [np.array(p) for p in lane.getShape(False)]
assert len(lane_shape) >= 2, repr(lane_shape)
heading = vec_to_radians(lane_shape[1] - lane_shape[0])
heading = Heading(heading)
orientation = fast_quaternion_from_angle(heading)
first_lanepoint = LinkedLanePoint(
lp=LanePoint(
lane=road_map.lane_by_id(lane.getID()),
pose=Pose(position=lane_shape[0],
orientation=orientation),
lane_width=road_map.lane_by_id(
lane.getID()).width_at_offset(0),
),
nexts=[],
is_inferred=False,
)
if previous_lp is not None:
previous_lp.nexts.append(first_lanepoint)
if initial_lanepoint is None:
initial_lanepoint = first_lanepoint
lanepoint_by_lane_memo[lane.getID()] = first_lanepoint
shape_lanepoints.append(first_lanepoint)
curr_lanepoint = first_lanepoint
for p1, p2 in zip(lane_shape[1:], lane_shape[2:]):
heading_ = vec_to_radians(p2 - p1)
heading_ = Heading(heading_)
orientation_ = fast_quaternion_from_angle(heading_)
linked_lanepoint = LinkedLanePoint(
lp=LanePoint(
lane=road_map.lane_by_id(lane.getID()),
pose=Pose(position=p1, orientation=orientation_),
lane_width=road_map.lane_by_id(
lane.getID()).width_at_offset(0),
),
nexts=[],
is_inferred=False,
)
shape_lanepoints.append(linked_lanepoint)
curr_lanepoint.nexts.append(linked_lanepoint)
curr_lanepoint = linked_lanepoint
# Add a lanepoint for the last point of the current lane
last_linked_lanepoint = LinkedLanePoint(
lp=LanePoint(
lane=curr_lanepoint.lp.lane,
pose=Pose(
position=lane_shape[-1],
orientation=curr_lanepoint.lp.pose.orientation,
),
lane_width=curr_lanepoint.lp.lane.width_at_offset(0),
),
nexts=[],
is_inferred=False,
)
shape_lanepoints.append(last_linked_lanepoint)
curr_lanepoint.nexts.append(last_linked_lanepoint)
curr_lanepoint = last_linked_lanepoint
for out_connection in lane.getOutgoing():
out_lane = out_connection.getToLane()
# Use internal lanes of junctions (if we're at a junction)
via_lane_id = out_connection.getViaLaneID()
if via_lane_id:
out_lane = road_map._graph.getLane(via_lane_id)
lane_queue.put((out_lane, curr_lanepoint))
return initial_lanepoint, shape_lanepoints
