def main():
parser = argparse.ArgumentParser(
description='Convert a base map from txt to bin format')
parser.add_argument(
'-i',
'--input_file',
help='Input base map in txt format',
type=str,
default='modules/map/data/gen/base_map.txt')
parser.add_argument(
'-o',
'--output_file',
help='Output base map in bin format',
type=str,
default='modules/map/data/gen/base_map.bin')
args = vars(parser.parse_args())
input_file_name = args['input_file']
output_file_name = args['output_file']
with open(input_file_name, 'r') as f:
mp = Map()
text_format.Merge(f.read(), mp)
# Output map
with open(output_file_name, "wb") as f:
f.write(mp.SerializeToString())
def main():
parser = argparse.ArgumentParser(
description=
'Generate Base Map from Recorded Localization and Mobileye Lane Detection'
)
parser.add_argument(
'-i',
'--input_file',
help='Recorded localization and mobileye lane detection in CSV format',
type=str,
default='/tmp/lane.csv')
parser.add_argument('--debug',
help='Print debugging info in /tmp',
action='store_true')
parser.add_argument('-o',
'--output_file',
help='Output file name of generated base map',
type=str,
default='modules/map/data/gen/base_map.txt')
parser.add_argument(
'-e',
'--end_waypoint_file',
help='Output file name of default end waypoint',
type=str,
default='modules/map/data/gen/default_end_way_point.txt')
parser.add_argument(
'--default_width',
help=
'Default lane width in meters (only effective when mobileye lane detection fails for ALL frames)',
type=float,
default=3.5)
parser.add_argument(
'--sample_distance',
help='minimum distance (in meters) of two adjacent samples of a lane',
type=float,
default=0.2)
parser.add_argument(
'--max_lane_length',
help=
'maximum length (in meters) of a lane (longer lanes will be split)',
type=float,
default=100.0)
parser.add_argument('--left_lanes',
help='Number of lanes on the left',
type=int,
default=0)
parser.add_argument('--right_lanes',
help='Number of lanes on the right',
type=int,
default=0)
args = vars(parser.parse_args())
csv_file_name = args['input_file']
map_file_name = args['output_file']
waypoint_file_name = args['end_waypoint_file']
default_width = args['default_width']
debug_option = args['debug']
sample_distance = args['sample_distance']
max_lane_length = args['max_lane_length']
left_lanes = args['left_lanes']
right_lanes = args['right_lanes']
default_ratio = 0.5
temp_csv_file_name = '/tmp/lane_interpolation.csv'
rows = []
with open(csv_file_name, 'r') as csvfile:
reader = csv.reader(csvfile)
for row in reader:
rows.append(row)
# Extract data samples
data = []
for row in rows[1:]:
entry = DataPoint()
entry.pos_x = float(row[0])
entry.pos_y = float(row[1])
entry.pos_z = float(row[2])
entry.theta = float(row[3])
entry.dist_left = abs(float(row[4]))
entry.conf_left = int(row[5])
if entry.dist_left < 0.1:
entry.conf_left = 0
entry.dist_right = abs(float(row[6]))
entry.conf_right = int(row[7])
if entry.dist_right < 0.1:
entry.conf_right = 0
entry.width = default_width
entry.ratio = default_ratio
data.append(entry)
# Fill in widths using interpolation
interpolate_width(data, default_width)
# Fill in ratios using interpolation
interpolate_ratio(data, default_ratio)
# Fill in centers
compute_center(data)
# Sample data at the interval of sample_distance
data = sample_data(data, sample_distance)
# Smooth center curves and widths
smooth_center_width(data)
# Output debug info if necessary
if debug_option:
with open(temp_csv_file_name, 'w') as csvfile:
for row in data:
csvfile.write(
"%s, %s, %s, %s, %s, %s, %s, %s, %s, %s, %s, %s\n" %
(row.pos_x, row.pos_y, row.pos_z, row.theta, row.dist_left,
row.conf_left, row.dist_right, row.conf_right, row.width,
row.ratio, row.center_x, row.center_y))
# Split data samples into lists with maximum length of max_lane_length
list_data = split_data(data, max_lane_length)
# Create individual lanes
lane_sets = []
for (lane_count, lane_data) in enumerate(list_data):
lane_set = []
for offset in range(-left_lanes, right_lanes + 1):
lane_set.append(
create_lane(lane_data, offset, lane_count, left_lanes,
right_lanes))
lane_sets.append(lane_set)
# Create road
road = create_road(data, left_lanes, right_lanes)
# Create map
mp = Map()
mp.header.version = "1.400000"
mp.header.date = "20170919"
mp.header.district = "101"
# Set up predecessors, successors, left/right neighbors
for lane_count in range(len(lane_sets)):
for lane_offset in range(len(lane_sets[lane_count])):
if lane_count != 0:
lane_sets[lane_count][lane_offset].predecessor_id.add(
).id = lane_sets[lane_count - 1][lane_offset].id.id
if lane_count != len(lane_sets) - 1:
lane_sets[lane_count][lane_offset].successor_id.add(
).id = lane_sets[lane_count + 1][lane_offset].id.id
if lane_offset != 0:
lane_sets[
lane_count][lane_offset].left_neighbor_forward_lane_id.add(
).id = lane_sets[lane_count][lane_offset - 1].id.id
if lane_offset != len(lane_sets[lane_count]) - 1:
lane_sets[lane_count][
lane_offset].right_neighbor_forward_lane_id.add(
).id = lane_sets[lane_count][lane_offset + 1].id.id
# Add road/lanes to map and let road contain lanes
mp.road.extend([road])
for lane_set in lane_sets:
for lane in lane_set:
mp.road[0].section[0].lane_id.add().id = lane.id.id
mp.lane.extend([lane])
# Output map
with open(map_file_name, "w") as f:
f.write(mp.__str__())
# Create default end_way_point using the farthest point of last central lane
last_central_lane = lane_sets[-1][left_lanes]
poi = POI()
landmark = poi.landmark.add()
landmark.name = "default"
waypoint = landmark.waypoint.add()
waypoint.id = last_central_lane.id.id
waypoint.s = last_central_lane.length
waypoint.pose.x = last_central_lane.central_curve.segment[
0].line_segment.point[-1].x
waypoint.pose.y = last_central_lane.central_curve.segment[
0].line_segment.point[-1].y
# Output default end_way_point
with open(waypoint_file_name, "w") as f:
f.write(poi.__str__())
def main():
parser = argparse.ArgumentParser(
description='Generate Base Map from Recorded Localization and Mobileye Lane Detection')
parser.add_argument(
'-i',
'--input_file',
help='Recorded localization and mobileye lane detection in CSV format',
type=str,
default='/tmp/lane.csv')
parser.add_argument(
'--debug',
help='Print debugging info in /tmp',
action='store_true')
parser.add_argument(
'-o',
'--output_file',
help='Output file name of generated base map',
type=str,
default='modules/map/data/gen/base_map.txt')
parser.add_argument(
'-e',
'--end_waypoint_file',
help='Output file name of default end waypoint',
type=str,
default='modules/map/data/gen/default_end_way_point.txt')
parser.add_argument(
'--default_width',
help='Default lane width in meters (only effective when mobileye lane detection fails for ALL frames)',
type=float,
default=3.5)
parser.add_argument(
'--sample_distance',
help='minimum distance (in meters) of two adjacent samples of a lane',
type=float,
default=0.2)
parser.add_argument(
'--max_lane_length',
help='maximum length (in meters) of a lane (longer lanes will be split)',
type=float,
default=100.0)
parser.add_argument(
'--left_lanes',
help='Number of lanes on the left',
type=int,
default=0)
parser.add_argument(
'--right_lanes',
help='Number of lanes on the right',
type=int,
default=0)
args = vars(parser.parse_args())
csv_file_name = args['input_file']
map_file_name = args['output_file']
waypoint_file_name = args['end_waypoint_file']
default_width = args['default_width']
debug_option = args['debug']
sample_distance = args['sample_distance']
max_lane_length = args['max_lane_length']
left_lanes = args['left_lanes']
right_lanes = args['right_lanes']
default_ratio = 0.5
temp_csv_file_name = '/tmp/lane_interpolation.csv'
rows = []
with open(csv_file_name, 'r') as csvfile:
reader = csv.reader(csvfile)
for row in reader:
rows.append(row)
# Extract data samples
data = []
for row in rows[1:]:
entry = DataPoint()
entry.pos_x = float(row[0])
entry.pos_y = float(row[1])
entry.pos_z = float(row[2])
entry.theta = float(row[3])
entry.dist_left = abs(float(row[4]))
entry.conf_left = int(row[5])
if entry.dist_left < 0.1:
entry.conf_left = 0
entry.dist_right = abs(float(row[6]))
entry.conf_right = int(row[7])
if entry.dist_right < 0.1:
entry.conf_right = 0
entry.width = default_width
entry.ratio = default_ratio
data.append(entry)
# Fill in widths using interpolation
interpolate_width(data, default_width)
# Fill in ratios using interpolation
interpolate_ratio(data, default_ratio)
# Fill in centers
compute_center(data)
# Sample data at the interval of sample_distance
data = sample_data(data, sample_distance)
# Smooth center curves and widths
smooth_center_width(data)
# Output debug info if necessary
if debug_option:
with open(temp_csv_file_name, 'w') as csvfile:
for row in data:
csvfile.write(
"%s, %s, %s, %s, %s, %s, %s, %s, %s, %s, %s, %s\n" %
(row.pos_x, row.pos_y, row.pos_z, row.theta, row.dist_left, row.conf_left, row.dist_right, row.conf_right, row.width, row.ratio, row.center_x, row.center_y))
# Split data samples into lists with maximum length of max_lane_length
list_data = split_data(data, max_lane_length)
# Create individual lanes
lane_sets = []
for (lane_count, lane_data) in enumerate(list_data):
lane_set = []
for offset in range(-left_lanes, right_lanes + 1):
lane_set.append(create_lane(lane_data, offset, lane_count, left_lanes, right_lanes))
lane_sets.append(lane_set)
# Create road
road = create_road(data, left_lanes, right_lanes)
# Create map
mp = Map()
mp.header.version = "1.400000"
mp.header.date = "20170919"
mp.header.district = "101"
# Set up predecessors, successors, left/right neighbors
for lane_count in range(len(lane_sets)):
for lane_offset in range(len(lane_sets[lane_count])):
if lane_count != 0:
lane_sets[lane_count][lane_offset].predecessor_id.add().id = lane_sets[lane_count - 1][lane_offset].id.id
if lane_count != len(lane_sets) - 1:
lane_sets[lane_count][lane_offset].successor_id.add().id = lane_sets[lane_count + 1][lane_offset].id.id
if lane_offset != 0:
lane_sets[lane_count][lane_offset].left_neighbor_forward_lane_id.add().id = lane_sets[lane_count][lane_offset - 1].id.id
if lane_offset != len(lane_sets[lane_count]) - 1:
lane_sets[lane_count][lane_offset].right_neighbor_forward_lane_id.add().id = lane_sets[lane_count][lane_offset + 1].id.id
# Add road/lanes to map and let road contain lanes
mp.road.extend([road])
for lane_set in lane_sets:
for lane in lane_set:
mp.road[0].section[0].lane_id.add().id = lane.id.id
mp.lane.extend([lane])
# Output map
with open(map_file_name, "w") as f:
f.write(mp.__str__())
# Create default end_way_point using the farthest point of last central lane
last_central_lane = lane_sets[-1][left_lanes]
poi = POI()
landmark = poi.landmark.add()
landmark.name = "default"
landmark.waypoint.id = last_central_lane.id.id
landmark.waypoint.s = last_central_lane.length
landmark.waypoint.pose.x = last_central_lane.central_curve.segment[0].line_segment.point[-1].x
landmark.waypoint.pose.y = last_central_lane.central_curve.segment[0].line_segment.point[-1].y
# Output default end_way_point
with open(waypoint_file_name, "w") as f:
f.write(poi.__str__())