def plot_vehicle(localization_pb, ax):
loc_x = [localization_pb.pose.position.x]
loc_y = [localization_pb.pose.position.y]
current_t = localization_pb.header.timestamp_sec
ax.plot(loc_x, loc_y, "bo")
position = []
position.append(localization_pb.pose.position.x)
position.append(localization_pb.pose.position.y)
position.append(localization_pb.pose.position.z)
mkz_polygon.plot(position, localization_pb.pose.heading, ax)
content = "t = " + str(current_t) + "\n"
content += "speed @y = " + \
str(localization_pb.pose.linear_velocity.y) + "\n"
content += "acc @y = " + \
str(localization_pb.pose.linear_acceleration_vrf.y)
lxy = [-80, 80]
ax.annotate(content,
xy=(loc_x[0], loc_y[0]),
xytext=lxy,
textcoords='offset points',
ha='left',
va='top',
bbox=dict(boxstyle='round,pad=0.5', fc='yellow', alpha=0.3),
arrowprops=dict(arrowstyle='->', connectionstyle='arc3,rad=0'),
alpha=0.8)
def plot_vehicle(localization_pb, ax):
loc_x = [localization_pb.pose.position.x]
loc_y = [localization_pb.pose.position.y]
current_t = localization_pb.header.timestamp_sec
ax.plot(loc_x, loc_y, "bo")
position = []
position.append(localization_pb.pose.position.x)
position.append(localization_pb.pose.position.y)
position.append(localization_pb.pose.position.z)
mkz_polygon.plot(position, localization_pb.pose.heading, ax)
content = "t = " + str(current_t) + "\n"
content += "speed @y = " + str(localization_pb.pose.linear_velocity.y) + "\n"
content += "acc @y = " + str(localization_pb.pose.linear_acceleration_vrf.y)
lxy = [-80, 80]
ax.annotate(
content,
xy=(loc_x[0], loc_y[0]),
xytext=lxy,
textcoords='offset points',
ha='left',
va='top',
bbox=dict(boxstyle='round,pad=0.5', fc='yellow', alpha=0.3),
arrowprops=dict(arrowstyle='->', connectionstyle='arc3,rad=0'),
alpha=0.8)
# plot car
loc_x = [localization_pb.pose.position.x]
loc_y = [localization_pb.pose.position.y]
current_t = localization_pb.header.timestamp_sec
plt.plot(loc_x, loc_y, "bo")
position = []
position.append(localization_pb.pose.position.x)
position.append(localization_pb.pose.position.y)
position.append(localization_pb.pose.position.z)
quaternion = []
quaternion.append(localization_pb.pose.orientation.qx)
quaternion.append(localization_pb.pose.orientation.qy)
quaternion.append(localization_pb.pose.orientation.qz)
quaternion.append(localization_pb.pose.orientation.qw)
mkz_polygon.plot(position, quaternion, plt)
content = "t = " + str(current_t) + "\n"
content += "speed @y = " + str(localization_pb.pose.linear_velocity.y) + "\n"
content += "acc @y = " + str(localization_pb.pose.linear_acceleration_vrf.y)
lxy = [-80, 80]
plt.annotate(content,
xy=(loc_x[0], loc_y[0]),
xytext=lxy,
textcoords='offset points',
ha='left',
va='top',
bbox=dict(boxstyle='round,pad=0.5', fc='yellow', alpha=0.3),
arrowprops=dict(arrowstyle='->', connectionstyle='arc3,rad=0'),
alpha=0.8)
# plot projected point
