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)
y = []
for obs_id, obs in mobileye.obstacles.items():
x.append(obs.x)
y.append(obs.y)
obstacles_points.set_xdata(x)
obstacles_points.set_ydata(y)
if fpath is not None:
px, py = fpath.get_xy()
path_line.set_xdata(px)
path_line.set_ydata(py)
fpath = None
ad_vehicle = ADVehicle()
routing = RoutingProvider()
mobileye = MobileyeProvider()
obs_decider = ObstacleDecider()
path_decider = PathDecider(True, False, False)
rospy.init_node("path_decider_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)
from provider_routing import RoutingProvider
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 = []