def init():
global planning_pub, log_file
global path_decider, speed_decider, traj_generator, obs_decider
global mobileye_provider, chassis_provider
global localization_provider, routing_provider
global adv
path_decider = PathDecider(FLAGS.enable_routing_aid, FLAGS.enable_nudge,
FLAGS.enable_change_lane)
speed_decider = SpeedDecider(FLAGS.max_cruise_speed, FLAGS.enable_follow)
traj_generator = TrajectoryGenerator()
mobileye_provider = MobileyeProvider()
routing_provider = RoutingProvider()
adv = ADVehicle()
obs_decider = ObstacleDecider()
pgm_path = os.path.dirname(os.path.realpath(__file__))
log_path = pgm_path + "/logs/"
if not os.path.exists(log_path):
os.makedirs(log_path)
now = datetime.datetime.now().strftime("%Y-%m-%d_%H.%M.%S")
log_file = open(log_path + now + ".txt", "w")
rospy.init_node(FLAGS.navigation_planning_node_name, anonymous=True)
rospy.Subscriber('/apollo/sensor/mobileye', mobileye_pb2.Mobileye,
mobileye_callback)
rospy.Subscriber('/apollo/localization/pose',
localization_pb2.LocalizationEstimate,
localization_callback)
rospy.Subscriber('/apollo/canbus/chassis', chassis_pb2.Chassis,
chassis_callback)
rospy.Subscriber('/apollo/navigation/routing', String, routing_callback)
planning_pub = rospy.Publisher(FLAGS.navigation_planning_topic,
planning_pb2.ADCTrajectory,
queue_size=1)
vehicle_point.set_xdata([ad_vehicle.x])
vehicle_point.set_ydata([ad_vehicle.y])
if ad_vehicle.is_ready():
path = routing.get_local_path(ad_vehicle.x, ad_vehicle.y,
ad_vehicle.heading)
path_x, path_y = path.get_xy()
local_line.set_xdata(path_x)
local_line.set_ydata(path_y)
ax.autoscale_view()
ax.relim()
# ax2.autoscale_view()
# ax2.relim()
ad_vehicle = ADVehicle()
routing = RoutingProvider()
rospy.init_node("routing_debug", anonymous=True)
rospy.Subscriber('/apollo/localization/pose',
localization_pb2.LocalizationEstimate,
localization_callback)
rospy.Subscriber('/apollo/navigation/routing', String, routing_callback)
rospy.Subscriber('/apollo/canbus/chassis', chassis_pb2.Chassis,
chassis_callback)
fig = plt.figure()
ax = plt.subplot2grid((3, 1), (0, 0), rowspan=2, colspan=1)
ax2 = plt.subplot2grid((3, 1), (2, 0), rowspan=1, colspan=1)
routing_line, = ax.plot([], [], 'r-')
vehicle_point, = ax.plot([], [], 'ko')