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)
planning_pub = None
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 = []