modified planning_traj_plot to use heading instead of quaternion

modules/tools/planning_traj_plot/example_data/1_localization.pb.txt

@@ -15,6 +15,7 @@ pose {
     qz: -0.990379842317655
     qw: -0.13821034037911459
   }
+  heading: -1.732
   linear_velocity {
     x: 0.00087471447426956314
     y: -0.0012088329450176513

modules/tools/planning_traj_plot/main.py

@@ -17,14 +17,41 @@
 ###############################################################################
 
 import sys
-sys.path.append("../../bazel-genfiles")
-from modules.planning.proto import planning_pb2
-from modules.localization.proto import localization_pb2
+import gflags
+from gflags import FLAGS
 import matplotlib.pyplot as plt
 from google.protobuf import text_format
 import mkz_polygon
-import gflags
-from gflags import FLAGS
+from modules.planning.proto import planning_pb2
+from modules.localization.proto import localization_pb2
+
+
+def read_planning_pb(planning_pb_file):
+    planning_pb = planning_pb2.ADCTrajectory()
+    f_handle = open(planning_pb_file, 'r')
+    text_format.Merge(f_handle.read(), planning_pb)
+    f_handle.close()
+    return planning_pb
+
+
+def read_localization_pb(localization_pb_file):
+    localization_pb = localization_pb2.LocalizationEstimate()
+    f_handle = open(localization_pb_file, 'r')
+    text_format.Merge(f_handle.read(), localization_pb)
+    f_handle.close()
+    return localization_pb
+
+
+def plot_trajectory(planning_pb, ax):
+    points_x = []
+    points_y = []
+    points_t = []
+    base_time_sec = planning_pb.header.timestamp_sec
+    for trajectory_point in planning_pb.adc_trajectory_point:
+        points_x.append(trajectory_point.x)
+        points_y.append(trajectory_point.y)
+        points_t.append(base_time_sec + trajectory_point.relative_time)
+    ax.plot(points_x, points_y, "r.")
 
 
 def find_closest_t(points_t, current_t):
@@ -45,82 +72,83 @@ def find_closest_t(points_t, current_t):
         return current_t
 
 
-if len(sys.argv) < 2:
-    print "usage: python main.py planning.pb.txt localization.pb.txt"
-    sys.exit()
-
-planning_pb_file = sys.argv[1]
-planning_pb = planning_pb2.ADCTrajectory()
-f_handle = open(planning_pb_file, 'r')
-text_format.Merge(f_handle.read(), planning_pb)
-f_handle.close()
-#print planning_pb_data
-
-localization_pb_file = sys.argv[2]
-localization_pb = localization_pb2.LocalizationEstimate()
-f_handle = open(localization_pb_file, 'r')
-text_format.Merge(f_handle.read(), localization_pb)
-f_handle.close()
-
-# plot traj
-points_x = []
-points_y = []
-points_t = []
-base_time_sec = planning_pb.header.timestamp_sec
-for trajectory_point in planning_pb.adc_trajectory_point:
-    points_x.append(trajectory_point.x)
-    points_y.append(trajectory_point.y)
-    points_t.append(base_time_sec + trajectory_point.relative_time)
-plt.plot(points_x, points_y, "r.")
-
-# 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
-matched_t = find_closest_t(points_t, current_t)
-idx = points_t.index(matched_t)
-plt.plot([points_x[idx]], [points_y[idx]], "bs")
-content = "t = " + str(matched_t) + "\n"
-content += "speed = " + str(trajectory_point.speed) + "\n"
-content += "acc = " + str(trajectory_point.acceleration_s)
-lxy = [-80, -80]
-plt.annotate(
-    content,
-    xy=(points_x[idx], points_y[idx]),
-    xytext=lxy,
-    textcoords='offset points',
-    ha='right',
-    va='top',
-    bbox=dict(boxstyle='round,pad=0.5', fc='yellow', alpha=0.3),
-    arrowprops=dict(arrowstyle='->', connectionstyle='arc3,rad=0'),
-    alpha=0.8)
-
-plt.axis('equal')
-plt.show()
+def find_closest_traj_point(planning_pb, current_t):
+    points_x = []
+    points_y = []
+    points_t = []
+    base_time_sec = planning_pb.header.timestamp_sec
+    for trajectory_point in planning_pb.adc_trajectory_point:
+        points_x.append(trajectory_point.x)
+        points_y.append(trajectory_point.y)
+        points_t.append(base_time_sec + trajectory_point.relative_time)
+
+    matched_t = find_closest_t(points_t, current_t)
+    idx = points_t.index(matched_t)
+    return planning_pb.adc_trajectory_point[idx]
+
+
+def plot_traj_point(planning_pb, traj_point, ax):
+    matched_t = planning_pb.header.timestamp_sec \
+                + traj_point.relative_time
+    ax.plot([traj_point.x], [traj_point.y], "bs")
+    content = "t = " + str(matched_t) + "\n"
+    content += "speed = " + str(traj_point.speed) + "\n"
+    content += "acc = " + str(traj_point.acceleration_s)
+    lxy = [-80, -80]
+    ax.annotate(
+        content,
+        xy=(traj_point.x, traj_point.y),
+        xytext=lxy,
+        textcoords='offset points',
+        ha='right',
+        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)
+
+if __name__ == "__main__":
+    if len(sys.argv) < 2:
+        print "usage: python main.py planning.pb.txt localization.pb.txt"
+        sys.exit()
+
+    planning_pb_file = sys.argv[1]
+    localization_pb_file = sys.argv[2]
+    planning_pb = read_planning_pb(planning_pb_file)
+    localization_pb = read_localization_pb(localization_pb_file)
+
+    plot_trajectory(planning_pb, plt)
+    plot_vehicle(localization_pb, plt)
+
+    current_t = localization_pb.header.timestamp_sec
+    trajectory_point = find_closest_traj_point(planning_pb, current_t)
+    plot_traj_point(planning_pb, trajectory_point, plt)
+
+    plt.axis('equal')
+    plt.show()

modules/tools/planning_traj_plot/mkz_polygon.py

@@ -16,80 +16,16 @@
 # limitations under the License.
 ###############################################################################
 
-import numpy
 import math
 
-_NEXT_AXIS = [1, 2, 0, 1]
-_EPS = numpy.finfo(float).eps * 4.0
 
+def get(position, heading):
 
-def _quaternion_matrix(quaternion):
-    q = numpy.array(quaternion, dtype=numpy.float64, copy=True)
-    n = numpy.dot(q, q)
-    if n < _EPS:
-        return numpy.identity(4)
-    q *= math.sqrt(2.0 / n)
-    q = numpy.outer(q, q)
-    return numpy.array(
-        [[1.0 - q[2, 2] - q[3, 3], q[1, 2] - q[3, 0], q[1, 3] + q[2, 0], 0.0],
-         [q[1, 2] + q[3, 0], 1.0 - q[1, 1] - q[3, 3], q[2, 3] - q[1, 0], 0.0],
-         [q[1, 3] - q[2, 0], q[2, 3] + q[1, 0], 1.0 - q[1, 1] - q[2, 2],
-          0.0], [0.0, 0.0, 0.0, 1.0]])
-
-
-def _euler_from_matrix(matrix):
-    firstaxis = 0
-    parity = 0
-    repetition = 0
-    frame = 0
-
-    i = firstaxis
-    j = _NEXT_AXIS[i + parity]
-    k = _NEXT_AXIS[i - parity + 1]
-
-    M = numpy.array(matrix, dtype=numpy.float64, copy=False)[:3, 0:3]
-    if repetition:
-        sy = math.sqrt(M[i, j] * M[i, j] + M[i, k] * M[i, k])
-        if sy > _EPS:
-            ax = math.atan2(M[i, j], M[i, k])
-            ay = math.atan2(sy, M[i, i])
-            az = math.atan2(M[j, i], -M[k, i])
-        else:
-            ax = math.atan2(-M[j, k], M[j, j])
-            ay = math.atan2(sy, M[i, i])
-            az = 0.0
-    else:
-        cy = math.sqrt(M[i, i] * M[i, i] + M[j, i] * M[j, i])
-        if cy > _EPS:
-            ax = math.atan2(M[k, j], M[k, k])
-            ay = math.atan2(-M[k, i], cy)
-            az = math.atan2(M[j, i], M[i, i])
-        else:
-            ax = math.atan2(-M[j, k], M[j, j])
-            ay = math.atan2(-M[k, i], cy)
-            az = 0.0
-
-    if parity:
-        ax, ay, az = -ax, -ay, -az
-    if frame:
-        ax, az = az, ax
-    return ax, ay, az
-
-
-def get(position, quaternion):
-    """
-    :param position: [x, y, z]
-    :param quaternion: [x, y, z, w]
-    :return:
-    """
     front_edge_to_center = 3.89
     back_edge_to_center = 1.043
     left_edge_to_center = 1.055
     right_edge_to_center = 1.055
 
-    x, y, z = _euler_from_matrix(_quaternion_matrix(quaternion))
-    heading = x + math.pi / 2.0
-
     cos_h = math.cos(heading)
     sin_h = math.sin(heading)
     #  (p3)  -------- (p0)

modules/tools/planning_traj_plot/run.sh

@@ -25,5 +25,5 @@ fi
 
 DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
 
-export PYTHONPATH="${DIR}/../../bazel-genfiles:${PYTHONPATH}"
+export PYTHONPATH="${DIR}/../../../bazel-genfiles:${PYTHONPATH}"
 eval "python ${DIR}/main.py $1 $2"