# import rospy
import rosbag
import matplotlib.pyplot as plt
sys.path.append('/opt/deeproute/control/lib/control_common_tmp/')
sys.path.append('/opt/deeproute/planner/lib/planner_common_tmp/')
sys.path.append('/opt/deeproute/sensors/lib/sensors')
sys.path.append('/opt/deeproute/sensors/lib/python2.7/dist-packages/sensors')
import car_state_pb2
import path_pb2
import car_info_pb2
import control_command_pb2
car_state = car_state_pb2.CarState()
path = path_pb2.Path()
car_info = car_info_pb2.CarInfo()
steer_report = car_info_pb2.SteerReport()
control_command = control_command_pb2.ControlCommand()
# *********************************************************
# *********************************************************
import os
# *********************************************************
# *********************************************************
# bag = rosbag.Bag('/home/dr/data/0712_test_chongqing/down1.bag')
for filename in os.listdir('/home/dr/license_data/intersection_right_turn'):
print filename
out_str = filename.split(".", 1)
print out_str
path_points = []
circles = []
fig = plt.gcf()
ax = fig.gca()
def get_color(idx):
colors = ['#ff3300', '#33cc33', '#0066ff', '#ffff66', '#9966ff']
return colors[idx % len(colors)]
for idx, path_file in enumerate(path_files):
f = open(path_file, 'rb')
message = text_format.Parse(f.read(), path_pb2.Path())
xs = []
ys = []
for p in message.path_waypoint_list:
xs.append(float(p.x))
ys.append(float(p.y))
cr = plt.Circle((p.x, p.y), 15, color=get_color(idx), alpha=0.5)
ax.add_artist(cr)
plt.plot(xs, ys, color=get_color(idx))
for idx, point_file in enumerate(point_files):
f = open(point_file, 'rb')
message = text_format.Parse(f.read(), point_obstacle_pb2.PointObstacle())
xs = []
ys = []
for p in message.point_list: