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