tools: added routing display in mobileye viewer.

modules/tools/mobileye_viewer/plot_mobileye.py

@@ -17,6 +17,7 @@
 ###############################################################################
 
 import rospy
+import json
 import argparse
 import matplotlib.pyplot as plt
 import matplotlib.animation as animation
@@ -25,6 +26,7 @@ from modules.drivers.proto import mobileye_pb2
 from mobileye_data import MobileyeData
 from localization_data import LocalizationData
 from planning_data import PlanningData
+from routing_data import RoutingData
 from chassis_data import ChassisData
 from view_subplot import ViewSubplot
 from subplot_s_speed import SubplotSSpeed
@@ -39,10 +41,11 @@ mobileye = MobileyeData()
 localization = LocalizationData()
 planning = PlanningData()
 chassis = ChassisData()
+routing_data = RoutingData()
 
 
 def update(frame_number):
-    view_subplot.show(mobileye, localization, planning, chassis)
+    view_subplot.show(mobileye, localization, planning, chassis, routing_data)
     s_speed_subplot.show(planning)
     s_theta_subplot.show(planning)
     s_time_subplot.show(planning)
@@ -70,7 +73,7 @@ def chassis_callback(chassis_pb):
 
 
 def routing_callback(routing_str):
-    pass
+    routing_data.update(routing_str)
 
 
 def add_listener():

modules/tools/mobileye_viewer/routing_data.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 threading
+import json
+
+
+class RoutingData:
+    def __init__(self, routing_str=None):
+        self.routing_str = routing_str
+        self.routing_data_lock = threading.Lock()
+        self.routing_x = []
+        self.routing_y = []
+
+    def update(self, routing_str):
+        self.routing_str = routing_str
+        routing_json = json.loads(routing_str.data)
+        routing_x = []
+        routing_y = []
+        for step in routing_json:
+            points = step['polyline']['points']
+            for point in points:
+                routing_x.append(point[0])
+                routing_y.append(point[1])
+
+        self.routing_data_lock.acquire()
+        self.routing_x = routing_x
+        self.routing_y = routing_y
+        self.routing_data_lock.release()

modules/tools/mobileye_viewer/view_subplot.py

@@ -33,6 +33,8 @@ class ViewSubplot:
         self.vehicle = ax.plot(
             [-1.055, 1.055, 1.055, -1.055, -1.055], [0, 0, -4.933, -4.933, 0],
             'r-', lw=1)
+        self.routing, = ax.plot(
+            [0], [0], 'r--', lw=3, alpha=0.8)
 
         self.speed_line, = ax.plot([0], [0], 'r-', lw=3, alpha=0.4)
         self.acc_line, = ax.plot([0], [0], 'y-', lw=3, alpha=1)
@@ -51,7 +53,7 @@ class ViewSubplot:
         self.ref_lane.set_visible(False)
 
     def show(self, mobileye_data, localization_data, planning_data,
-             chassis_data):
+             chassis_data, routing_data):
         self.left_lane.set_visible(True)
         self.right_lane.set_visible(True)
         self.ref_lane.set_visible(True)
@@ -89,6 +91,34 @@ class ViewSubplot:
 
         if localization_data.localization_pb is None:
             return
+
+        vx = localization_data.localization_pb.pose.position.x
+        vy = localization_data.localization_pb.pose.position.y
+
+        routing_data.routing_data_lock.acquire()
+        path_x = [x - vx for x in routing_data.routing_x]
+        path_y = [y - vy for y in routing_data.routing_y]
+        routing_data.routing_data_lock.release()
+
+        heading = localization_data.localization_pb.pose.heading
+        npath_x = []
+        npath_y = []
+
+        for i in range(len(path_x)):
+            x = path_x[i]
+            y = path_y[i]
+            # newx = x * math.cos(heading) - y * math.sin(heading)
+            # newy = y * math.cos(heading) + x * math.sin(heading)
+            newx = x * math.cos(- heading + 1.570796) - y * math.sin(
+                -heading + 1.570796)
+            newy = y * math.cos(- heading + 1.570796) + x * math.sin(
+                -heading + 1.570796)
+            npath_x.append(newx)
+            npath_y.append(newy)
+
+        self.routing.set_xdata(npath_x)
+        self.routing.set_ydata(npath_y)
+
         speed_x = localization_data.localization_pb.pose.linear_velocity.x
         speed_y = localization_data.localization_pb.pose.linear_velocity.y
         acc_x = localization_data.localization_pb.pose.linear_acceleration.x