def init():
global planning_pub, log_file
global path_decider, speed_decider, traj_generator
global mobileye_provider, chassis_provider
global localization_provider, routing_provider
path_decider = PathDecider()
speed_decider = SpeedDecider(FLAGS.max_cruise_speed)
traj_generator = TrajectoryGenerator()
mobileye_provider = MobileyeProvider()
chassis_provider = ChassisProvider()
localization_provider = LocalizationProvider()
routing_provider = RoutingProvider()
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)
left_lm.set_ydata(left_y)
right_path = mobileye.get_right_lane_marker_path()
right_x, right_y = right_path.get_xy()
right_lm.set_xdata(right_x)
right_lm.set_ydata(right_y)
middle_path = mobileye.get_lane_marker_middle_path(128)
middle_x, middle_y = middle_path.get_xy()
middle_lm.set_xdata(middle_x)
middle_lm.set_ydata(middle_y)
# ax.autoscale_view()
# ax.relim()
ad_vehicle = ADVehicle()
routing = RoutingProvider()
mobileye = MobileyeProvider()
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)
rospy.Subscriber('/apollo/sensor/mobileye', mobileye_pb2.Mobileye,
mobileye_callback)
fig = plt.figure()
ax = plt.subplot2grid((1, 1), (0, 0), rowspan=1, colspan=1)
left_lm, = ax.plot([], [], 'b-')
routing_debug_pub = None
PUB_NODE_NAME = "planning"
PUB_TOPIC = "/apollo/" + PUB_NODE_NAME
CRUISE_SPEED = 10 # m/s
ENABLE_FOLLOW = False
ENABLE_ROUTING_AID = False
f = open("benchmark.txt", "w")
path_decider = PathDecider()
speed_decider = SpeedDecider()
traj_generator = TrajectoryGenerator()
mobileye_provider = MobileyeProvider()
chassis_provider = ChassisProvider()
localization_provider = LocalizationProvider()
routing_provider = RoutingProvider()
nx = []
ny = []
local_seg_x = []
local_seg_y = []
local_smooth_seg_x = []
local_smooth_seg_y = []
left_marker_x = []
left_marker_y = []
right_marker_x = []
right_marker_y = []