def create_lane(data, offset, lane_count, left_lanes, right_lanes):
"""
create a lane using 'data' whose lateral index is 'offset'
offset = 0: center lane; offset < 0: left lanes; offset > 0: right lanes
lane_count: longitutional index of lane (used for naming)
left_lanes, right_lanes: number of left/right lanes (used for boundary types)
"""
total_length = 0.0
total_left_length = 0.0
total_right_length = 0.0
lane = Lane()
lane.id.id = "lane_" + str(lane_count) + "_" + str(offset)
lane_central_curve_seg = lane.central_curve.segment.add()
start_heading = data[0].theta
lane_left_boundary_curve_seg = lane.left_boundary.curve.segment.add()
lane_left_boundary_curve_seg.heading = float(start_heading)
lane_left_boundary_curve_seg.s = 0.0
lane_right_boundary_curve_seg = lane.right_boundary.curve.segment.add()
lane_right_boundary_curve_seg.heading = float(start_heading)
lane_right_boundary_curve_seg.s = 0.0
last_l_x = 0.0
last_l_y = 0.0
last_c_x = 0.0
last_c_y = 0.0
last_r_x = 0.0
last_r_y = 0.0
for (index, entry) in enumerate(data):
theta = entry.theta
theta_left = theta + np.pi / 2.0
theta_right = theta - np.pi / 2.0
pos_c_x = entry.center_x
pos_c_y = entry.center_y
pos_l_x = pos_c_x + entry.width * (0.5 - offset) * np.cos(theta_left)
pos_l_y = pos_c_y + entry.width * (0.5 - offset) * np.sin(theta_left)
pos_r_x = pos_c_x + entry.width * (0.5 + offset) * np.cos(theta_right)
pos_r_y = pos_c_y + entry.width * (0.5 + offset) * np.sin(theta_right)
pos_c_x = (pos_l_x + pos_r_x) / 2.0
pos_c_y = (pos_l_y + pos_r_y) / 2.0
if index == 0:
lane_central_curve_seg.start_position.x = pos_c_x
lane_central_curve_seg.start_position.y = pos_c_y
lane_left_boundary_curve_seg.start_position.x = pos_l_x
lane_left_boundary_curve_seg.start_position.y = pos_l_y
lane_right_boundary_curve_seg.start_position.x = pos_r_x
lane_right_boundary_curve_seg.start_position.y = pos_r_y
else:
d = distance(last_c_x, last_c_y, pos_c_x, pos_c_y)
total_length += d
d_left = distance(last_l_x, last_l_y, pos_l_x, pos_l_y)
total_left_length += d_left
d_right = distance(last_r_x, last_r_y, pos_r_x, pos_r_y)
total_right_length += d_right
point = lane_central_curve_seg.line_segment.point.add()
point.x = pos_c_x
point.y = pos_c_y
point = lane_left_boundary_curve_seg.line_segment.point.add()
point.x = pos_l_x
point.y = pos_l_y
point = lane_right_boundary_curve_seg.line_segment.point.add()
point.x = pos_r_x
point.y = pos_r_y
sample = lane.left_sample.add()
sample.s = total_length
sample.width = entry.width / 2.0
sample = lane.right_sample.add()
sample.s = total_length
sample.width = entry.width / 2.0
last_l_x = pos_l_x
last_l_y = pos_l_y
last_r_x = pos_r_x
last_r_y = pos_r_y
last_c_x = pos_c_x
last_c_y = pos_c_y
lane_central_curve_seg.length = total_length
lane_left_boundary_curve_seg.length = total_left_length
lane_right_boundary_curve_seg.length = total_right_length
boundary_type = lane.left_boundary.boundary_type.add()
boundary_type.s = 0.0
if offset == -left_lanes:
boundary_type.types.append(LaneBoundaryType.DOUBLE_YELLOW)
else:
boundary_type.types.append(LaneBoundaryType.DOTTED_WHITE)
lane.left_boundary.length = total_left_length
boundary_type = lane.right_boundary.boundary_type.add()
boundary_type.s = 0.0
if offset == right_lanes:
boundary_type.types.append(LaneBoundaryType.CURB)
else:
boundary_type.types.append(LaneBoundaryType.DOTTED_WHITE)
lane.right_boundary.length = total_right_length
lane.length = total_length
lane.speed_limit = 29.06
lane.type = Lane.CITY_DRIVING
lane.turn = Lane.NO_TURN
return lane
def create_lane(data, offset, lane_count, left_lanes, right_lanes):
"""
create a lane using 'data' whose lateral index is 'offset'
offset = 0: center lane; offset < 0: left lanes; offset > 0: right lanes
lane_count: longitutional index of lane (used for naming)
left_lanes, right_lanes: number of left/right lanes (used for boundary types)
"""
total_length = 0.0
total_left_length = 0.0
total_right_length = 0.0
lane = Lane()
lane.id.id = "lane_" + str(lane_count) + "_" + str(offset)
lane_central_curve_seg = lane.central_curve.segment.add()
start_heading = data[0].theta
lane_left_boundary_curve_seg = lane.left_boundary.curve.segment.add()
lane_left_boundary_curve_seg.heading = float(start_heading)
lane_left_boundary_curve_seg.s = 0.0
lane_right_boundary_curve_seg = lane.right_boundary.curve.segment.add()
lane_right_boundary_curve_seg.heading = float(start_heading)
lane_right_boundary_curve_seg.s = 0.0
last_l_x = 0.0
last_l_y = 0.0
last_c_x = 0.0
last_c_y = 0.0
last_r_x = 0.0
last_r_y = 0.0
for (index, entry) in enumerate(data):
theta = entry.theta
theta_left = theta + np.pi / 2.0
theta_right = theta - np.pi / 2.0
pos_c_x = entry.center_x
pos_c_y = entry.center_y
pos_l_x = pos_c_x + entry.width * (0.5 - offset) * np.cos(theta_left)
pos_l_y = pos_c_y + entry.width * (0.5 - offset) * np.sin(theta_left)
pos_r_x = pos_c_x + entry.width * (0.5 + offset) * np.cos(theta_right)
pos_r_y = pos_c_y + entry.width * (0.5 + offset) * np.sin(theta_right)
pos_c_x = (pos_l_x + pos_r_x) / 2.0
pos_c_y = (pos_l_y + pos_r_y) / 2.0
if index == 0:
lane_central_curve_seg.start_position.x = pos_c_x
lane_central_curve_seg.start_position.y = pos_c_y
lane_left_boundary_curve_seg.start_position.x = pos_l_x
lane_left_boundary_curve_seg.start_position.y = pos_l_y
lane_right_boundary_curve_seg.start_position.x = pos_r_x
lane_right_boundary_curve_seg.start_position.y = pos_r_y
else:
d = distance(last_c_x, last_c_y, pos_c_x, pos_c_y)
total_length += d
d_left = distance(last_l_x, last_l_y, pos_l_x, pos_l_y)
total_left_length += d_left
d_right = distance(last_r_x, last_r_y, pos_r_x, pos_r_y)
total_right_length += d_right
point = lane_central_curve_seg.line_segment.point.add()
point.x = pos_c_x
point.y = pos_c_y
point = lane_left_boundary_curve_seg.line_segment.point.add()
point.x = pos_l_x
point.y = pos_l_y
point = lane_right_boundary_curve_seg.line_segment.point.add()
point.x = pos_r_x
point.y = pos_r_y
sample = lane.left_sample.add()
sample.s = total_length
sample.width = entry.width / 2.0
sample = lane.right_sample.add()
sample.s = total_length
sample.width = entry.width / 2.0
last_l_x = pos_l_x
last_l_y = pos_l_y
last_r_x = pos_r_x
last_r_y = pos_r_y
last_c_x = pos_c_x
last_c_y = pos_c_y
lane_central_curve_seg.length = total_length
lane_left_boundary_curve_seg.length = total_left_length
lane_right_boundary_curve_seg.length = total_right_length
boundary_type = lane.left_boundary.boundary_type.add()
boundary_type.s = 0.0
if offset == -left_lanes:
boundary_type.types.append(LaneBoundaryType.DOUBLE_YELLOW)
else:
boundary_type.types.append(LaneBoundaryType.DOTTED_WHITE)
lane.left_boundary.length = total_left_length
boundary_type = lane.right_boundary.boundary_type.add()
boundary_type.s = 0.0
if offset == right_lanes:
boundary_type.types.append(LaneBoundaryType.CURB)
else:
boundary_type.types.append(LaneBoundaryType.DOTTED_WHITE)
lane.right_boundary.length = total_right_length
lane.length = total_length
lane.speed_limit = 29.06
lane.type = Lane.CITY_DRIVING
lane.turn = Lane.NO_TURN
return lane