Add routing debugging tools, Fix topo graph output format (#1104)

* Fix topo graph generation file format

* Add python tools for debugging routing

modules/routing/tools/debug_passage_region.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 itertools
+import matplotlib.pyplot as plt
+import debug_topo
+import gen.topo_graph_pb2 as topo_graph_pb2
+import gen.router_pb2 as router_pb2
+
+color_iter = itertools.cycle(
+    ['navy', 'c', 'cornflowerblue', 'gold', 'darkorange'])
+g_central_curve_dict = {}
+g_center_point_dict = {}
+
+
+def get_center_of_passage_region(region):
+    """Get center of passage region center curve"""
+    center_points = []
+    for seg in region.segment:
+        center_points.append(g_center_point_dict[seg.id])
+    return center_points[len(center_points) // 2]
+
+
+def read_routing_result(file_name):
+    """Read routing result"""
+    fin = open(file_name)
+    result = router_pb2.RoutingResult()
+    result.ParseFromString(fin.read())
+    return result
+
+
+def plot_region(region, color):
+    "Plot passage region"
+    for seg in region.segment:
+        center_pt = debug_topo.plot_central_curve_with_s_range(
+            g_central_curve_dict[seg.id], seg.start_s, seg.end_s, color=color)
+        debug_topo.draw_id(center_pt, seg.id, 'r')
+        g_center_point_dict[seg.id] = center_pt
+        print 'Plot lane id: %s, start s: %f, end s: %f' % (seg.id, seg.start_s,
+                                                            seg.end_s)
+
+
+def plot_lane_change(lane_change, passage_regions):
+    """Plot lane change infomation"""
+    st_idx = lane_change.start_passage_region_index
+    ed_idx = lane_change.end_passage_region_index
+    from_pt = get_center_of_passage_region(passage_regions[st_idx])
+    to_pt = get_center_of_passage_region(passage_regions[ed_idx])
+    plt.gca().annotate(
+        "",
+        xy=(to_pt[0], to_pt[1]),
+        xytext=(from_pt[0], from_pt[1]),
+        arrowprops=dict(
+            facecolor='blue', edgecolor='none', alpha=0.7, shrink=0.05))
+
+
+def plot_road(road):
+    """Plot road"""
+    for region in road.passage_region:
+        plot_region(region, 'green')
+    for lane_change in road.lane_change_info:
+        plot_lane_change(lane_change, road.passage_region)
+
+
+def plot_junction(junction):
+    """Plot junction"""
+    plot_region(junction.passage_region, 'red')
+
+
+def plot_result(routing_result, central_curve_dict):
+    """Plot routing result"""
+    plt.close()
+    plt.figure()
+    for way in routing_result.route:
+        if way.HasField("road_info"):
+            plot_road(way.road_info)
+        else:
+            plot_junction(way.junction_info)
+
+    plt.gca().set_aspect(1)
+    plt.title('Passage region')
+    plt.xlabel('x')
+    plt.ylabel('y')
+    plt.legend()
+
+    plt.draw()
+
+
+def print_help():
+    """Print help information.
+
+    Print help information of usage.
+
+    Args:
+
+    """
+    print 'usage:'
+    print '     python debug_topo.py file_path, then',
+    print_help_command()
+
+
+def print_help_command():
+    """Print command help information.
+
+    Print help information of command.
+
+    Args:
+
+    """
+    print 'type in command: [q] [r]'
+    print '         q               exit'
+    print '         p               plot passage region'
+
+
+if __name__ == '__main__':
+    if len(sys.argv) != 3:
+        print_help()
+        sys.exit(0)
+    print 'Please wait for loading data...'
+
+    file_name = sys.argv[1]
+    fin = open(file_name)
+    graph = topo_graph_pb2.Graph()
+    graph.ParseFromString(fin.read())
+    for nd in graph.node:
+        g_central_curve_dict[nd.lane_id] = nd.central_curve
+
+    plt.ion()
+    while 1:
+        print_help_command()
+        print 'cmd>',
+        instruction = raw_input()
+        argv = instruction.strip(' ').split(' ')
+        if len(argv) == 1:
+            if argv[0] == 'q':
+                sys.exit(0)
+            elif argv[0] == 'p':
+                result = read_routing_result(sys.argv[2])
+                plot_result(result, g_central_curve_dict)
+            else:
+                print '[ERROR] wrong command'
+            continue
+
+        else:
+            print '[ERROR] wrong arguments'
+            continue

modules/routing/tools/debug_route.py

@@ -20,11 +20,12 @@ import sys
 import itertools
 import matplotlib.pyplot as plt
 import debug_topo
-import gen.topo_graph_pb2 as topo_graph_pb2
+import modules.routing.proto.topo_graph_pb2 as topo_graph_pb2
 import plot_map
 
-color_iter = itertools.cycle(['navy', 'c', 'cornflowerblue', 'gold',
-                              'darkorange'])
+color_iter = itertools.cycle(
+    ['navy', 'c', 'cornflowerblue', 'gold', 'darkorange'])
+
 
 def read_route(route_file_name):
     """Read route result text file"""
@@ -51,10 +52,12 @@ def plot_route(lanes, central_curve_dict):
             color = 'red'
         else:
             color = 'green'
-        debug_topo.plot_central_curve_with_s_range(central_curve_dict[lane_id],
-                                                   lane['start s'],
-                                                   lane['end s'],
-                                                   color=color)
+        mid_pt = debug_topo.plot_central_curve_with_s_range(
+            central_curve_dict[lane_id],
+            lane['start s'],
+            lane['end s'],
+            color=color)
+        debug_topo.draw_id(mid_pt, lane_id, 'y')
     plt.gca().set_aspect(1)
     plt.title('Routing result')
     plt.xlabel('x')

modules/routing/tools/debug_topo.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 math
+import itertools
+import matplotlib.pyplot as plt
+import modules.routing.proto.topo_graph_pb2 as topo_graph_pb2
+import plot_map
+import plot_event
+
+color_iter = itertools.cycle(
+    ['navy', 'c', 'cornflowerblue', 'gold', 'darkorange'])
+
+
+def downsample_array(array):
+    """down sample given array"""
+    skip = 5
+    result = array[::skip]
+    result.append(array[-1])
+    return result
+
+
+def calculate_s(px, py):
+    """Calculate s array based on x and y arrays"""
+    dis = 0.0
+    ps = [dis]
+    for i in range(len(px) - 1):
+        gap = math.sqrt(pow(px[i + 1] - px[i], 2) + pow(py[i + 1] - py[i], 2))
+        dis = dis + gap
+        ps.append(dis)
+    return ps
+
+
+def extract_line(line):
+    """extract line, return x array and y array"""
+    px = []
+    py = []
+    for pt in line.point:
+        px.append(float(pt.x))
+        py.append(float(pt.y))
+    return px, py
+
+
+def draw_line(line, color):
+    """draw line, return x array and y array"""
+    px = []
+    py = []
+    for pt in line.point:
+        px.append(float(pt.x))
+        py.append(float(pt.y))
+    px = downsample_array(px)
+    py = downsample_array(py)
+    plt.gca().plot(px, py, color=color, lw=3, alpha=0.8)
+    return px, py
+
+
+def draw_arc(arc):
+    """draw arc"""
+    xy = (arc.center.x, arc.center.y)
+    start = 0
+    end = 0
+    if arc.start_angle < arc.end_angle:
+        start = arc.start_angle / math.pi * 180
+        end = arc.end_angle / math.pi * 180
+    else:
+        end = arc.start_angle / math.pi * 180
+        start = arc.end_angle / math.pi * 180
+
+    pac = mpatches.Arc(
+        xy, arc.radius * 2, arc.radius * 2, angle=0, theta1=start, theta2=end)
+
+    plt.gca().add_patch(pac)
+
+
+def draw_id(coordinate, id_string, color):
+    """draw id"""
+    x = coordinate[0]
+    y = coordinate[1]
+    plt.annotate(
+        id_string,
+        xy=(x, y),
+        xytext=(30, 30),
+        textcoords='offset points',
+        bbox=dict(boxstyle='round,pad=0.5', fc=color, alpha=0.5),
+        arrowprops=dict(arrowstyle='->', connectionstyle='arc3,rad=0'),
+        horizontalalignment='right',
+        verticalalignment='bottom')
+
+
+def plot_central_curve_with_s_range(central_curve, start_s, end_s, color):
+    """plot topology graph node with given start and end s, return middle point"""
+    node_x = []
+    node_y = []
+    plot_length = 0.0
+    for curve in central_curve.segment:
+        px, py = extract_line(curve.line_segment)
+        node_x = node_x + px
+        node_y = node_y + py
+    start_plot_index = 0
+    end_plot_index = len(node_x)
+    node_s = calculate_s(node_x, node_y)
+    for i in range(len(node_s)):
+        if node_s[i] >= start_s:
+            start_plot_index = i
+            break
+    for i in range(len(node_s) - 1, -1, -1):
+        if node_s[i] <= end_s:
+            end_plot_index = i + 1
+            break
+    plt.gca().plot(
+        node_x[start_plot_index:end_plot_index],
+        node_y[start_plot_index:end_plot_index],
+        color=color,
+        lw=3,
+        alpha=0.8)
+    mid_index = (start_plot_index + end_plot_index) // 2
+    return [node_x[mid_index], node_y[mid_index]]
+
+
+def plot_central_curve(central_curve, color):
+    """plot topology graph node, return node middle point"""
+    node_x = []
+    node_y = []
+    for curve in central_curve.segment:
+        if curve.HasField('line_segment'):
+            px, py = draw_line(curve.line_segment, color)
+            node_x = node_x + px
+            node_y = node_y + py
+        #if curve.HasField('arc'):
+        #    draw_arc(curve.arc)
+    return [node_x[len(node_x) / 2], node_y[len(node_y) / 2]]
+
+
+def plot_node(node, plot_id, color):
+    """plot topology graph node"""
+    print 'length of %s: %f' % (node.lane_id, node.length)
+    mid_pt = plot_central_curve(node.central_curve, color)
+    if 'l' in plot_id:
+        draw_id(mid_pt, node.lane_id, 'green')
+    if 'r' in plot_id:
+        draw_id(mid_pt, node.road_id, 'red')
+    return mid_pt
+
+
+def plot_edge(edge, midddle_point_map):
+    """plot topology graph edge"""
+    if edge.direction_type == topo_graph_pb2.Edge.FORWARD:
+        return
+    # if lane change is allowed, draw an arrow from lane with from_id to lane with to_id
+    from_id = edge.from_lane_id
+    from_pt = midddle_point_map[from_id]
+    to_id = edge.to_lane_id
+    to_pt = midddle_point_map[to_id]
+    plt.gca().annotate(
+        "",
+        xy=(to_pt[0], to_pt[1]),
+        xytext=(from_pt[0], from_pt[1]),
+        arrowprops=dict(arrowstyle="->", connectionstyle="arc3"))
+
+
+def plot_all(graph, plot_id=''):
+    """plot topology graph"""
+    plt.close()
+    fig = plt.figure()
+    fig.canvas.mpl_connect('button_press_event', plot_event.onclick)
+    lane_middle_point_map = {}
+    for i, (nd, color) in enumerate(zip(graph.node, color_iter)):
+        nd_mid_pt = plot_node(nd, plot_id, color)
+        lane_middle_point_map[nd.lane_id] = nd_mid_pt
+    for i, eg in enumerate(graph.edge):
+        plot_edge(eg, lane_middle_point_map)
+    plt.gca().set_aspect(1)
+    plt.title('Routing topology graph')
+    plt.xlabel('x')
+    plt.ylabel('y')
+    plt.legend()
+
+    plt.draw()
+
+
+def plot_id(graph, lane_id):
+    """plot topology graph"""
+    plt.close()
+    fig = plt.figure()
+    fig.canvas.mpl_connect('button_press_event', plot_event.onclick)
+    lane_middle_point_map = {}
+    plot_ids = [lane_id]
+    for eg in graph.edge:
+        if eg.from_lane_id == lane_id:
+            plot_ids.append(eg.to_lane_id)
+    for i, (nd, color) in enumerate(zip(graph.node, color_iter)):
+        if nd.lane_id in plot_ids:
+            nd_mid_pt = plot_node(nd, 'l', color)
+            lane_middle_point_map[nd.lane_id] = nd_mid_pt
+    for i, eg in enumerate(graph.edge):
+        if eg.from_lane_id == lane_id:
+            plot_edge(eg, lane_middle_point_map)
+    plt.gca().set_aspect(1)
+    plt.title('Routing topology graph')
+    plt.xlabel('x')
+    plt.ylabel('y')
+    plt.legend()
+
+    plt.draw()
+
+
+def print_help():
+    """Print help information.
+
+    Print help information of usage.
+
+    Args:
+
+    """
+    print 'usage:'
+    print '     python debug_topo.py file_path, then',
+    print_help_command()
+
+
+def print_help_command():
+    """Print command help information.
+
+    Print help information of command.
+
+    Args:
+
+    """
+    print 'type in command: [q] [a] [i lane_id]'
+    print '         q               exit'
+    print '         a               plot all topology'
+    print '         a_id            plot all topology with lane id'
+    print '         a_rid           plot all topology with road id'
+    print '         i lane_id       plot lanes could be reached from lane with lane_id'
+    print '         i_map lane_id   plot lanes could be reached from lane with lane_id, with map'
+
+
+if __name__ == '__main__':
+    if len(sys.argv) != 2:
+        print_help()
+        sys.exit(0)
+    print 'Please wait for loading data...'
+
+    map_path = '/home/caros/adu_data/map/base_map.bin'
+    file_name = sys.argv[1]
+    fin = open(file_name)
+    graph = topo_graph_pb2.Graph()
+    graph.ParseFromString(fin.read())
+    print "district: %s" % graph.hdmap_district
+    print "version: %s" % graph.hdmap_version
+
+    plt.ion()
+    while 1:
+        print_help_command()
+        print 'cmd>',
+        instruction = raw_input()
+        argv = instruction.strip(' ').split(' ')
+        if len(argv) == 1:
+            if argv[0] == 'q':
+                sys.exit(0)
+            elif argv[0] == 'a':
+                plot_all(graph)
+            elif argv[0] == 'a_id':
+                plot_all(graph, 'l')
+            elif argv[0] == 'a_rid':
+                plot_all(graph, 'r')
+            else:
+                print '[ERROR] wrong command'
+            continue
+
+        if len(argv) == 2:
+            if argv[0] == 'i':
+                plot_id(graph, argv[1])
+            elif argv[0] == 'i_map':
+                plot_id(graph, argv[1])
+                plot_map.draw_map(plt.gca(), map_path)
+            else:
+                print '[ERROR] wrong command'
+            continue
+
+        else:
+            print '[ERROR] wrong arguments'
+            continue

modules/routing/tools/plot_event.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.
+###############################################################################
+
+
+def onclick(event):
+    """Event function when mouse left button is clicked"""
+    print '\nClick captured! x=%f\ty=%f' % (event.xdata, event.ydata)
+    print 'cmd>',

modules/routing/tools/plot_map.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 modules.map.proto.map_pb2 as map_pb2
+import matplotlib.pyplot as plt
+
+
+def load_pb_from_file(filename, pb_value):
+    """load pb from file"""
+    try:
+        f_handle = open(filename, "rb")
+        pb_value.ParseFromString(f_handle.read())
+        f_handle.close()
+    except Exception as e:
+        f_handle = open(filename, 'r')
+        text_format.Merge(f_handle.read(), pb_value)
+        f_handle.close()
+    return pb_value
+
+
+def downsample_array(array):
+    """down sample given array"""
+    skip = 5
+    result = array[::skip]
+    result.append(array[-1])
+    return result
+
+
+def draw_boundary(ax, line_segment):
+    """
+    :param line_segment:
+    :return:
+    """
+    px = []
+    py = []
+    for p in line_segment.point:
+        px.append(float(p.x))
+        py.append(float(p.y))
+    px = downsample_array(px)
+    py = downsample_array(py)
+    ax.plot(px, py, 'k', lw=0.4)
+
+
+def draw_map(ax, mapfile):
+    """ draw map from mapfile"""
+    drivemap = load_pb_from_file(mapfile, map_pb2.Map())
+
+    for lane in drivemap.lane:
+        for curve in lane.left_boundary.curve.segment:
+            if curve.HasField('line_segment'):
+                draw_boundary(ax, curve.line_segment)
+
+        for curve in lane.right_boundary.curve.segment:
+            if curve.HasField('line_segment'):
+                draw_boundary(ax, curve.line_segment)
+    plt.draw()
+
+    return drivemap

modules/routing/tools/road_show.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 modules.map.proto.map_pb2 as map_pb2
+import matplotlib.pyplot as plt
+
+g_color = [
+    'navy', 'c', 'cornflowerblue', 'gold', 'darkorange', 'darkviolet',
+    'aquamarine', 'firebrick', 'limegreen'
+]
+
+
+def draw_line(line_segment, color):
+    """
+    :param line_segment:
+    :return: none
+    """
+    px = []
+    py = []
+    for p in line_segment.point:
+        px.append(float(p.x))
+        py.append(float(p.y))
+    px = downsample_array(px)
+    py = downsample_array(py)
+    plt.gca().plot(px, py, lw=10, alpha=0.8, color=color)
+    return px[len(px) // 2], py[len(py) // 2]
+
+
+def draw_arc(arc):
+    """
+    :param arc: proto obj
+    :return: none
+    """
+    xy = (arc.center.x, arc.center.y)
+    start = 0
+    end = 0
+    if arc.start_angle < arc.end_angle:
+        start = arc.start_angle / math.pi * 180
+        end = arc.end_angle / math.pi * 180
+    else:
+        end = arc.start_angle / math.pi * 180
+        start = arc.end_angle / math.pi * 180
+
+    pac = mpatches.Arc(
+        xy, arc.radius * 2, arc.radius * 2, angle=0, theta1=start, theta2=end)
+
+    plt.gca().add_patch(pac)
+
+
+def downsample_array(array):
+    """down sample given array"""
+    skip = 5
+    result = array[::skip]
+    result.append(array[-1])
+    return result
+
+
+def draw_boundary(line_segment):
+    """
+    :param line_segment:
+    :return:
+    """
+    px = []
+    py = []
+    for p in line_segment.point:
+        px.append(float(p.x))
+        py.append(float(p.y))
+    px = downsample_array(px)
+    py = downsample_array(py)
+    plt.gca().plot(px, py, 'k')
+
+
+def draw_id(x, y, id_string):
+    """Draw id_string on (x, y)"""
+    plt.annotate(
+        id_string,
+        xy=(x, y),
+        xytext=(40, -40),
+        textcoords='offset points',
+        ha='right',
+        va='bottom',
+        bbox=dict(boxstyle='round,pad=0.5', fc='green', alpha=0.5),
+        arrowprops=dict(arrowstyle='->', connectionstyle='arc3,rad=0'))
+
+
+def get_road_index_of_lane(lane_id, road_lane_set):
+    """Get road index of lane"""
+    for i, lane_set in enumerate(road_lane_set):
+        if lane_id in lane_set:
+            return i
+    return -1
+
+
+def draw_map(drivemap):
+    """ draw map from mapfile"""
+    print 'Map info:'
+    print '\tVersion:\t',
+    print drivemap.header.version
+    print '\tDate:\t',
+    print drivemap.header.date
+    print '\tDistrict:\t',
+    print drivemap.header.district
+
+    road_lane_set = []
+    for road in drivemap.road:
+        lanes = []
+        for sec in road.section:
+            for lane in sec.lane_id:
+                lanes.append(lane.id)
+        road_lane_set.append(lanes)
+
+    for lane in drivemap.lane:
+        #print lane.type
+        #print lane.central_curve
+        #break
+        #print [f.name for f in lane.central_curve.DESCRIPTOR.fields]
+        for curve in lane.central_curve.segment:
+            if curve.HasField('line_segment'):
+                road_idx = get_road_index_of_lane(lane.id.id, road_lane_set)
+                if road_idx == -1:
+                    print 'Failed to get road index of lane'
+                    sys.exit(-1)
+                center_x, center_y = draw_line(curve.line_segment,
+                                               g_color[road_idx % len(g_color)])
+                draw_id(center_x, center_y, str(road_idx))
+                #break
+            if curve.HasField('arc'):
+                draw_arc(curve.arc)
+                #print "arc"
+
+        for curve in lane.left_boundary.curve.segment:
+            if curve.HasField('line_segment'):
+                draw_boundary(curve.line_segment)
+
+        for curve in lane.right_boundary.curve.segment:
+            if curve.HasField('line_segment'):
+                draw_boundary(curve.line_segment)
+                #break
+
+    return drivemap
+
+
+def print_help():
+    """Print help information.
+
+    Print help information of usage.
+
+    Args:
+
+    """
+    print 'usage:'
+    print '     python road_show.py base_map_file_path',
+
+
+if __name__ == "__main__":
+    if len(sys.argv) != 2:
+        print 'Wrong number of arguments.'
+        print_help()
+        sys.exit(0)
+    fin = open(sys.argv[1])
+    base_map = map_pb2.Map()
+    base_map.ParseFromString(fin.read())
+
+    plt.subplots()
+    draw_map(base_map)
+    plt.axis('equal')
+    plt.show()

modules/routing/topo_creator/graph_creator.cc

@@ -34,6 +34,8 @@ using ::apollo::routing::Edge;
 using ::apollo::hdmap::LaneBoundary;
 using ::apollo::hdmap::LaneBoundaryType;
 
+using ::apollo::common::util::EndWith;
+
 namespace {
 
 bool IsAllowedToCross(const LaneBoundary& boundary) {
@@ -123,12 +125,27 @@ bool GraphCreator::Create() {
     AddEdge(from_node, lane.successor_id(), Edge::FORWARD);
   }
 
+  if(!EndWith(dump_topo_file_path_, ".bin") && 
+     !EndWith(dump_topo_file_path_, ".txt")){
+    AERROR << "Failed to dump topo data into file, incorrect file type " 
+           << dump_topo_file_path_;
+    return false;
+  }
+  int type_pos = dump_topo_file_path_.find_last_of(".") + 1;
+  std::string bin_file = dump_topo_file_path_.replace(type_pos, 3, "bin");
+  std::string txt_file = dump_topo_file_path_.replace(type_pos, 3, "txt");
   if (!::apollo::common::util::SetProtoToASCIIFile(graph_,
-                                                   dump_topo_file_path_)) {
-    AERROR << "Failed to dump topo data into file " << dump_topo_file_path_;
+                                                   txt_file)) {
+    AERROR << "Failed to dump topo data into file " << txt_file;
+    return false;
+  }
+  AINFO << "Txt file is dumped successfully. Path: " << txt_file;
+  if (!::apollo::common::util::SetProtoToBinaryFile(graph_,
+                                                   bin_file)) {
+    AERROR << "Failed to dump topo data into file " << bin_file;
     return false;
   }
-  AINFO << "File is dumped successfully. Path: " << dump_topo_file_path_;
+  AINFO << "Bin file is dumped successfully. Path: " << bin_file;
   return true;
 }
 

modules/routing/topo_creator/topo_creator.cc

@@ -29,6 +29,7 @@ int main(int argc, char **argv) {
   creator_ptr.reset(new ::apollo::routing::GraphCreator(
       apollo::hdmap::BaseMapFile(), routing_map_file));
   CHECK(creator_ptr->Create()) << "Create routing topo failed!";
-  AINFO << "Create routing topo successfully from " << routing_map_file;
+  AINFO << "Create routing topo successfully from " 
+        << apollo::hdmap::BaseMapFile();
   return 0;
 }