planning: added tool to plot planning result

modules/planning/integration_tests/planning_test_base.cc

@@ -145,6 +145,9 @@ bool PlanningTestBase::RunPlanning(const std::string& test_case_name,
         AERROR << "Failed to write to file " << tmp_fname;
       }
       AERROR << "found\ndiff " << tmp_fname << " " << full_golden_path;
+      AERROR << "visualize diff\n python "
+                "modules/tools/plot_trace/plot_planning_result.py "
+             << tmp_fname << " " << full_golden_path;
       return false;
     }
   }

modules/tools/plot_trace/plot_planning_result.py

+#!/usr/bin/env python
+
+###############################################################################
+# Copyright 2017 The Apollo Authors. All Rights Reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+###############################################################################
+
+import sys
+
+import matplotlib
+import matplotlib.animation as animation
+import matplotlib.pyplot as plt
+from subprocess import call
+import numpy as np
+import rosbag
+import rospy
+from std_msgs.msg import String
+from google.protobuf import text_format
+from mpl_toolkits.mplot3d import Axes3D
+
+from modules.canbus.proto import chassis_pb2
+from modules.localization.proto import localization_pb2
+from modules.planning.proto import planning_pb2
+
+g_args = None
+
+
+def plot_planning(ax, planning_file):
+    fhandle = file(planning_file, 'r')
+    if not fhandle:
+        print "Failed to open file %s" % (planning_file)
+    planning_pb = planning_pb2.ADCTrajectory()
+    text_format.Merge(fhandle.read(), planning_pb)
+    x = [p.path_point.x for p in planning_pb.trajectory_point]
+    y = [p.path_point.y for p in planning_pb.trajectory_point]
+    z = [p.v for p in planning_pb.trajectory_point]
+    ax.plot(x, y, z, label=planning_file)
+    ax.legend()
+
+
+def press_key(event):
+    files = " ".join(g_args.planning_files)
+    if evnet.key == 'f':
+        call(["cp", files])
+
+
+if __name__ == '__main__':
+    import argparse
+    parser = argparse.ArgumentParser(
+        description=
+        """A visualization tool that can plot one or multiple planning "
+        results, so that we can compare the differences.
+        Example: plot_planning_result.py result_file1.pb.txt result_file2.pb.txt"""
+    )
+    parser.add_argument(
+        "planning_files",
+        action='store',
+        nargs="+",
+        help="The planning results")
+    g_args = parser.parse_args()
+
+    matplotlib.rcParams['legend.fontsize'] = 10
+    fig = plt.figure()
+    fig.canvas.mpl_connect('key_press_event', press_key)
+    ax = fig.gca(projection='3d')
+    ax.set_xlabel("x")
+    ax.set_ylabel("y")
+    ax.set_zlabel("speed")
+
+    for planning_file in g_args.planning_files:
+        plot_planning(ax, planning_file)
+
+    plt.show()

modules/tools/replay/replay_file.py

@@ -33,6 +33,7 @@ from modules.planning.proto import planning_pb2
 from modules.prediction.proto import prediction_obstacle_pb2
 from modules.routing.proto import routing_pb2
 
+
 def generate_message(topic, filename):
     """generate message from file"""
     message = None
@@ -61,34 +62,39 @@ def topic_publisher(topic, filename, period):
     """publisher"""
     rospy.init_node('replay_node', anonymous=True)
     pub = rospy.Publisher(topic, String, queue_size=1)
-    rate = rospy.Rate(int(1.0 / period))  # 10hz
+    rate = rospy.Rate(int(1.0 / period))
     message = generate_message(topic, filename)
     while not rospy.is_shutdown():
         pub.publish(str(message))
         rate.sleep()
 
+
 def seq_publisher(seq_num, period):
     """publisher"""
     rospy.init_node('replay_node', anonymous=True)
 
     # topic_name => module_name, pb type, pb, publish_handler
     topic_name_map = {
-        "/apollo/localization/pose" : ["localization",
-            localization_pb2.LocalizationEstimate, None, None],
-        "/apollo/routing_response" : ["routing",
-            routing_pb2.RoutingResponse, None, None],
-        "/apollo/perception/obstacles" : ["perception",
-            perception_obstacle_pb2.PerceptionObstacles, None, None],
-        "/apollo/prediction" : ["prediction",
-            prediction_obstacle_pb2.PredictionObstacles, None, None],
-        "/apollo/planning" : ["planning",
-            planning_pb2.ADCTrajectory, None, None],
+        "/apollo/localization/pose":
+        ["localization", localization_pb2.LocalizationEstimate, None, None],
+        "/apollo/routing_response":
+        ["routing", routing_pb2.RoutingResponse, None, None],
+        "/apollo/perception/obstacles": [
+            "perception", perception_obstacle_pb2.PerceptionObstacles, None,
+            None
+        ],
+        "/apollo/prediction": [
+            "prediction", prediction_obstacle_pb2.PredictionObstacles, None,
+            None
+        ],
+        "/apollo/planning":
+        ["planning", planning_pb2.ADCTrajectory, None, None],
     }
     for topic, module_features in topic_name_map.iteritems():
         filename = str(seq_num) + "_" + module_features[0] + ".pb.txt"
         print "trying to load pb file:", filename
-        module_features[3] = rospy.Publisher(topic,
-            module_features[1], queue_size=1)
+        module_features[3] = rospy.Publisher(
+            topic, module_features[1], queue_size=1)
         module_features[2] = generate_message(topic, filename)
         if module_features[2] is None:
             print topic, " pb is none"
@@ -105,7 +111,7 @@ if __name__ == '__main__':
     parser = argparse.ArgumentParser(
         description="replay a planning result pb file")
     parser.add_argument(
-        "--filename", action="store", type=str, help="planning result files")
+        "filename", action="store", type=str, help="planning result files")
     parser.add_argument(
         "--topic",
         action="store",