def plot_central_curve_with_s_range(central_curve, start_s, end_s, color):
"""plot topology graph node with given start and end s, return middle point"""
node_x = []
node_y = []
for curve in central_curve.segment:
px, py = proto_utils.flatten(curve.line_segment.point, ['x', 'y'])
node_x.extend(px)
node_y.extend(py)
start_plot_index = 0
end_plot_index = len(node_x)
node_s = calculate_s(node_x, node_y)
for i in range(len(node_s)):
if node_s[i] >= start_s:
start_plot_index = i
break
for i in range(len(node_s) - 1, -1, -1):
if node_s[i] <= end_s:
end_plot_index = i + 1
break
plt.gca().plot(
node_x[start_plot_index:end_plot_index],
node_y[start_plot_index:end_plot_index],
color=color,
lw=3,
alpha=0.8)
mid_index = (start_plot_index + end_plot_index) // 2
return [node_x[mid_index], node_y[mid_index]]
def plot_central_curve_with_s_range(central_curve, start_s, end_s, color):
"""plot topology graph node with given start and end s, return middle point"""
node_x = []
node_y = []
for curve in central_curve.segment:
px, py = proto_utils.flatten(curve.line_segment.point, ['x', 'y'])
node_x.extend(px)
node_y.extend(py)
start_plot_index = 0
end_plot_index = len(node_x)
node_s = calculate_s(node_x, node_y)
for i in range(len(node_s)):
if node_s[i] >= start_s:
start_plot_index = i
break
for i in range(len(node_s) - 1, -1, -1):
if node_s[i] <= end_s:
end_plot_index = i + 1
break
plt.gca().plot(node_x[start_plot_index:end_plot_index],
node_y[start_plot_index:end_plot_index],
color=color,
lw=3,
alpha=0.8)
mid_index = (start_plot_index + end_plot_index) // 2
return [node_x[mid_index], node_y[mid_index]]
def draw_boundary(line_segment):
"""
:param line_segment:
:return:
"""
px, py = proto_utils.flatten(line_segment.point, ['x', 'y'])
px, py = downsample_array(px), downsample_array(py)
plt.gca().plot(px, py, 'k')
def draw_boundary(line_segment):
"""
:param line_segment:
:return:
"""
px, py = proto_utils.flatten(line_segment.point, ['x', 'y'])
px, py = downsample_array(px), downsample_array(py)
plt.gca().plot(px, py, 'k')
def draw_line(line_segment, color):
"""
:param line_segment:
:return: none
"""
px, py = proto_utils.flatten(line_segment.point, ['x', 'y'])
px, py = downsample_array(px), downsample_array(py)
plt.gca().plot(px, py, lw=10, alpha=0.8, color=color)
return px[len(px) // 2], py[len(py) // 2]
def draw_line(line_segment, color):
"""
:param line_segment:
:return: none
"""
px, py = proto_utils.flatten(line_segment.point, ['x', 'y'])
px, py = downsample_array(px), downsample_array(py)
plt.gca().plot(px, py, lw=10, alpha=0.8, color=color)
return px[len(px) // 2], py[len(py) // 2]
def callback_planning(self, data):
"""New Planning Trajectory"""
if self.stgraph:
st_s, st_t, polygons_s, polygons_t = proto_utils.flatten(
data.debug.planning_data.st_graph,
['speed_profile.s',
'speed_profile.t',
'boundary.point.s',
'boundary.point.t'])
with self.lock:
for i in range(len(st_s)):
self.ax[i].new_planning(st_t[i], st_s[i],
polygons_t[i], polygons_s[i])
else:
if len(data.trajectory_point) == 0:
print data
return
x, y, speed, theta, kappa, acc, relative_time = np.array(
proto_utils.flatten(data.trajectory_point,
['path_point.x',
'path_point.y',
'v',
'path_point.theta',
'path_point.kappa',
'a',
'relative_time']))
relative_time += data.header.timestamp_sec
with self.lock:
self.ax[0].new_planning(relative_time, x, y)
self.ax[1].new_planning(relative_time, speed)
if self.ax[2].title == "Curvature":
self.ax[2].new_planning(relative_time, kappa)
if self.ax[3].title == "Heading":
self.ax[3].new_planning(relative_time, theta)
else:
self.ax[3].new_planning(relative_time, acc)
def callback_planning(self, data):
"""New Planning Trajectory"""
if self.stgraph:
st_s, st_t, polygons_s, polygons_t = proto_utils.flatten(
data.debug.planning_data.st_graph,
['speed_profile.s',
'speed_profile.t',
'boundary.point.s',
'boundary.point.t'])
with self.lock:
for i in range(len(st_s)):
self.ax[i].new_planning(st_t[i], st_s[i],
polygons_t[i], polygons_s[i])
else:
if len(data.trajectory_point) == 0:
print data
return
x, y, speed, theta, kappa, acc, relative_time = np.array(
proto_utils.flatten(data.trajectory_point,
['path_point.x',
'path_point.y',
'v',
'path_point.theta',
'path_point.kappa',
'a',
'relative_time']))
relative_time += data.header.timestamp_sec
with self.lock:
self.ax[0].new_planning(relative_time, x, y)
self.ax[1].new_planning(relative_time, speed)
if self.ax[2].title == "Curvature":
self.ax[2].new_planning(relative_time, kappa)
if self.ax[3].title == "Heading":
self.ax[3].new_planning(relative_time, theta)
else:
self.ax[3].new_planning(relative_time, acc)
def draw_map(drivemap):
""" draw map from mapfile"""
print('Map info:')
print('\tVersion:\t')
print(drivemap.header.version)
print('\tDate:\t')
print(drivemap.header.date)
print('\tDistrict:\t')
print(drivemap.header.district)
road_lane_set = []
for road in drivemap.road:
lanes = []
for sec in road.section:
lanes.extend(proto_utils.flatten(sec.lane_id, 'id'))
road_lane_set.append(lanes)
for lane in drivemap.lane:
for curve in lane.central_curve.segment:
if curve.HasField('line_segment'):
road_idx = get_road_index_of_lane(lane.id.id, road_lane_set)
if road_idx == -1:
print('Failed to get road index of lane')
sys.exit(-1)
center_x, center_y = draw_line(curve.line_segment,
g_color[road_idx % len(g_color)])
draw_id(center_x, center_y, str(road_idx))
#break
#if curve.HasField('arc'):
# draw_arc(curve.arc)
for curve in lane.left_boundary.curve.segment:
if curve.HasField('line_segment'):
draw_boundary(curve.line_segment)
for curve in lane.right_boundary.curve.segment:
if curve.HasField('line_segment'):
draw_boundary(curve.line_segment)
#break
return drivemap
def draw_map(drivemap):
""" draw map from mapfile"""
print 'Map info:'
print '\tVersion:\t',
print drivemap.header.version
print '\tDate:\t',
print drivemap.header.date
print '\tDistrict:\t',
print drivemap.header.district
road_lane_set = []
for road in drivemap.road:
lanes = []
for sec in road.section:
lanes.extend(proto_utils.flatten(sec.lane_id, 'id'))
road_lane_set.append(lanes)
for lane in drivemap.lane:
for curve in lane.central_curve.segment:
if curve.HasField('line_segment'):
road_idx = get_road_index_of_lane(lane.id.id, road_lane_set)
if road_idx == -1:
print 'Failed to get road index of lane'
sys.exit(-1)
center_x, center_y = draw_line(curve.line_segment,
g_color[road_idx % len(g_color)])
draw_id(center_x, center_y, str(road_idx))
#break
#if curve.HasField('arc'):
# draw_arc(curve.arc)
for curve in lane.left_boundary.curve.segment:
if curve.HasField('line_segment'):
draw_boundary(curve.line_segment)
for curve in lane.right_boundary.curve.segment:
if curve.HasField('line_segment'):
draw_boundary(curve.line_segment)
#break
return drivemap
def callback_planning(data):
if len(data.trajectory_point) == 0:
print data
return
x, y, speed, theta, kappa, acc, relative_time = np.array(
proto_utils.flatten(data.trajectory_point, [
'path_point.x', 'path_point.y', 'v', 'path_point.theta',
'path_point.kappa', 'a', 'relative_time'
]))
theta = theta * 180 / 3.1415926
relative_time += data.header.timestamp_sec
#plt.clf()
#plt.plot(-y,x)
#plt.pause(0.01)
global new_planning
new_planning = True
global planning_xy
planning_xy = []
planning_xy.append(-y)
planning_xy.append(x)
def draw_line(line, color):
"""draw line, return x array and y array"""
px, py = proto_utils.flatten(line.point, ['x', 'y'])
px, py = util.downsample_array(px), util.downsample_array(py)
plt.gca().plot(px, py, color=color, lw=3, alpha=0.8)
return px, py
def draw_line(line, color):
"""draw line, return x array and y array"""
px, py = proto_utils.flatten(line.point, ['x', 'y'])
px, py = util.downsample_array(px), util.downsample_array(py)
plt.gca().plot(px, py, color=color, lw=3, alpha=0.8)
return px, py
