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)
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)
obstacles_points, = ax.plot([], [], 'ro')
path_line, = ax.plot([], [], 'b-')
import time
import matplotlib.pyplot as plt
import matplotlib.animation as animation
from modules.drivers.proto import mobileye_pb2
from modules.planning.proto import planning_pb2
from modules.canbus.proto import chassis_pb2
from path_decider import PathDecider
from trajectory_generator import TrajectoryGenerator
planning_pub = None
PUB_NODE_NAME = "planning"
PUB_TOPIC = "/apollo/" + PUB_NODE_NAME
f = open("benchmark.txt", "w")
CRUISE_SPEED = 10 # m/s
path_decider = PathDecider()
traj_generator = TrajectoryGenerator()
nx = []
ny = []
def localization_callback(localization_pb):
speed_x = localization_pb.pose.linear_velocity.x
speed_y = localization_pb.pose.linear_velocity.y
acc_x = localization_pb.linear_acceleration.x
acc_y = localization_pb.linear_acceleration.y
def chassis_callback(chassis_pb):
global SPEED