def mobileye_callback(mobileye_pb):
global x, y, nx, ny
start_timestamp = time.time()
path_length = MINIMUM_PATH_LENGTH
if path_length < SPEED * 2:
path_length = math.ceil(SPEED * 2)
x, y = get_central_line(mobileye_pb, path_length)
nx, ny = get_path(x, y, path_length)
adc_trajectory = planning_pb2.ADCTrajectory()
adc_trajectory.header.timestamp_sec = rospy.get_rostime().secs
adc_trajectory.header.module_name = "planning"
adc_trajectory.gear = chassis_pb2.Chassis.GEAR_DRIVE
adc_trajectory.latency_stats.total_time_ms = (time.time() -
start_timestamp) * 1000
for i in range(len(nx)):
traj_point = adc_trajectory.trajectory_point.add()
traj_point.path_point.x = nx[i]
traj_point.path_point.y = ny[i]
traj_point.path_point.theta = 0 ## TODO
traj_point.path_point.s = 0 ##TODO
traj_point.v = 0 #TODO
traj_point.relative_time = 0 #TODO
planning_pub.publish(adc_trajectory)
f.write("duration: " + str(time.time() - start_timestamp) + "\n")
def generate(self, path, final_path_length, speed, start_timestamp):
path_x, path_y = path.get_xy()
adc_trajectory = planning_pb2.ADCTrajectory()
adc_trajectory.header.timestamp_sec = cyber_time.Time.now().to_sec()
adc_trajectory.header.module_name = "planning"
adc_trajectory.gear = chassis_pb2.Chassis.GEAR_DRIVE
adc_trajectory.latency_stats.total_time_ms = \
(cyber_time.Time.now().to_sec() - start_timestamp) * 1000
s = 0
relative_time = 0
adc_trajectory.engage_advice.advice \
= drive_state_pb2.EngageAdvice.READY_TO_ENGAGE
for x in range(int(final_path_length - 1)):
y = path_y[x]
traj_point = adc_trajectory.trajectory_point.add()
traj_point.path_point.x = x
traj_point.path_point.y = y
if x > 0:
dist = euclidean_distance((x, y), (x - 1, path_y[x - 1]))
s += dist
relative_time += dist / speed
traj_point.path_point.theta = get_theta((x + 1, path_y[x + 1]),
(0, path_y[0]))
traj_point.path_point.s = s
traj_point.v = speed
traj_point.relative_time = relative_time
return adc_trajectory
def mobileye_callback(mobileye_pb):
global x, y, nx ,ny
start_timestamp = time.time()
path_length = MINIMUM_PATH_LENGTH
if path_length < SPEED * 2:
path_length = math.ceil(SPEED * 2)
x, y = get_central_line(mobileye_pb, path_length)
nx, ny = x, y#get_path(x, y, path_length)
adc_trajectory = planning_pb2.ADCTrajectory()
adc_trajectory.header.timestamp_sec = rospy.get_rostime().secs
adc_trajectory.header.module_name = "planning"
adc_trajectory.gear = chassis_pb2.Chassis.GEAR_DRIVE
adc_trajectory.latency_stats.total_time_ms = (time.time() - start_timestamp) * 1000
s = 0
relative_time = 0
for i in range(len(nx)):
traj_point = adc_trajectory.trajectory_point.add()
traj_point.path_point.x = nx[i]
traj_point.path_point.y = ny[i]
if i > 0:
dist = euclidean_distance((nx[i], ny[i]), (nx[i-1], ny[i-1]))
s += dist
relative_time += dist / CRUISE_SPEED
traj_point.path_point.theta = get_theta((nx[i], ny[i]), (nx[0], ny[0]))
if i == 0:
traj_point.path_point.theta = 0
traj_point.path_point.s = s
traj_point.v = CRUISE_SPEED
traj_point.relative_time = relative_time
planning_pub.publish(adc_trajectory)
f.write("duration: " + str(time.time() - start_timestamp) + "\n")
def process_record(cls, input_record):
channel_size_stats = {}
freader = RecordReader(input_record)
print('----- Begin to process record -----')
for channelname, msg, datatype, timestamp in freader.read_messages():
if channelname in ChannelSizeStats.TOPICS:
if channelname in channel_size_stats:
channel_size_stats[channelname]['total'] += len(msg)
channel_size_stats[channelname]['num'] += 1
else:
channel_size_stats[channelname] = {}
channel_size_stats[channelname]['total'] = len(msg)
channel_size_stats[channelname]['num'] = 1.0
elif channelname == "/apollo/planning":
adc_trajectory = planning_pb2.ADCTrajectory()
adc_trajectory.ParseFromString(msg)
name = "planning_no_debug"
adc_trajectory.ClearField("debug")
planning_str = adc_trajectory.SerializeToString()
if name in channel_size_stats:
channel_size_stats[name]['total'] += len(planning_str)
channel_size_stats[name]['num'] += 1
else:
channel_size_stats[name] = {}
channel_size_stats[name]['total'] = len(planning_str)
channel_size_stats[name]['num'] = 1.0
for channelname in channel_size_stats.keys():
print(channelname, " num:", channel_size_stats[channelname]['num'],
" avg size:", channel_size_stats[channelname]['total'] / channel_size_stats[channelname]['num'])
print('----- Finish processing record -----')
def generate(self, nx, ny, speed, start_timestamp):
adc_trajectory = planning_pb2.ADCTrajectory()
adc_trajectory.header.timestamp_sec = rospy.Time.now().to_sec()
adc_trajectory.header.module_name = "planning"
adc_trajectory.gear = chassis_pb2.Chassis.GEAR_DRIVE
adc_trajectory.latency_stats.total_time_ms = \
(time.time() - start_timestamp) * 1000
s = 0
relative_time = 0
for i in range(len(nx)):
traj_point = adc_trajectory.trajectory_point.add()
traj_point.path_point.x = ny[i]
traj_point.path_point.y = -nx[i]
if i > 0:
dist = euclidean_distance((nx[i], ny[i]),
(nx[i - 1], ny[i - 1]))
s += dist
relative_time += dist / speed
if (i + 1) >= len(nx):
traj_point.path_point.theta = get_theta(
(nx[-1], ny[-1]), (nx[0], ny[0]))
else:
traj_point.path_point.theta = get_theta(
(nx[i + 1], ny[i + 1]), (nx[0], ny[0]))
traj_point.path_point.s = s
traj_point.v = speed
traj_point.relative_time = relative_time
return adc_trajectory
def process(control_analyzer, planning_analyzer, lidar_endtoend_analyzer):
is_auto_drive = False
for msg in reader.read_messages():
if msg.topic == "/apollo/canbus/chassis":
chassis = chassis_pb2.Chassis()
chassis.ParseFromString(msg.message)
if chassis.driving_mode == \
chassis_pb2.Chassis.COMPLETE_AUTO_DRIVE:
is_auto_drive = True
else:
is_auto_drive = False
if msg.topic == "/apollo/control":
if not is_auto_drive:
continue
control_cmd = control_cmd_pb2.ControlCommand()
control_cmd.ParseFromString(msg.message)
control_analyzer.put(control_cmd)
lidar_endtoend_analyzer.put_control(control_cmd)
if msg.topic == "/apollo/planning":
if not is_auto_drive:
continue
adc_trajectory = planning_pb2.ADCTrajectory()
adc_trajectory.ParseFromString(msg.message)
planning_analyzer.put(adc_trajectory)
lidar_endtoend_analyzer.put_planning(adc_trajectory)
if msg.topic == "/apollo/sensor/velodyne64/compensator/PointCloud2":
if not is_auto_drive:
continue
point_cloud = pointcloud_pb2.PointCloud()
point_cloud.ParseFromString(msg.message)
lidar_endtoend_analyzer.put_lidar(point_cloud)
def generate(self, path_coef, final_path_length, speed, start_timestamp):
adc_trajectory = planning_pb2.ADCTrajectory()
adc_trajectory.header.timestamp_sec = rospy.Time.now().to_sec()
adc_trajectory.header.module_name = "planning"
adc_trajectory.gear = chassis_pb2.Chassis.GEAR_DRIVE
adc_trajectory.latency_stats.total_time_ms = \
(time.time() - start_timestamp) * 1000
s = 0
relative_time = 0
for x in range(int(final_path_length)):
y = polyval(x, path_coef)
traj_point = adc_trajectory.trajectory_point.add()
traj_point.path_point.x = x
traj_point.path_point.y = -y
if x > 0:
dist = euclidean_distance((x, y),
(x - 1, polyval(x - 1, path_coef)))
s += dist
relative_time += dist / speed
traj_point.path_point.theta = get_theta(
(x + 1, polyval(x + 1, path_coef)), (0, polyval(0, path_coef)))
traj_point.path_point.s = s
traj_point.v = speed
traj_point.relative_time = relative_time
return adc_trajectory
def process(control_analyzer, planning_analyzer, lidar_endtoend_analyzer,
is_simulation, plot_planning_path, plot_planning_refpath, all_data):
is_auto_drive = False
for msg in reader.read_messages():
if msg.topic == "/apollo/canbus/chassis":
chassis = chassis_pb2.Chassis()
chassis.ParseFromString(msg.message)
if chassis.driving_mode == \
chassis_pb2.Chassis.COMPLETE_AUTO_DRIVE:
is_auto_drive = True
else:
is_auto_drive = False
if msg.topic == "/apollo/control":
if (not is_auto_drive and not all_data) or \
is_simulation or plot_planning_path or plot_planning_refpath:
continue
control_cmd = control_cmd_pb2.ControlCommand()
control_cmd.ParseFromString(msg.message)
control_analyzer.put(control_cmd)
lidar_endtoend_analyzer.put_pb('control', control_cmd)
if msg.topic == "/apollo/planning":
if (not is_auto_drive) and (not all_data):
continue
adc_trajectory = planning_pb2.ADCTrajectory()
adc_trajectory.ParseFromString(msg.message)
planning_analyzer.put(adc_trajectory)
lidar_endtoend_analyzer.put_pb('planning', adc_trajectory)
if plot_planning_path:
planning_analyzer.plot_path(plt, adc_trajectory)
if plot_planning_refpath:
planning_analyzer.plot_refpath(plt, adc_trajectory)
if msg.topic == "/apollo/sensor/velodyne64/compensator/PointCloud2" or \
msg.topic == "/apollo/sensor/lidar128/compensator/PointCloud2":
if ((not is_auto_drive) and (not all_data)) or is_simulation or \
plot_planning_path or plot_planning_refpath:
continue
point_cloud = pointcloud_pb2.PointCloud()
point_cloud.ParseFromString(msg.message)
lidar_endtoend_analyzer.put_lidar(point_cloud)
if msg.topic == "/apollo/perception/obstacles":
if ((not is_auto_drive) and (not all_data)) or is_simulation or \
plot_planning_path or plot_planning_refpath:
continue
perception = perception_obstacle_pb2.PerceptionObstacles()
perception.ParseFromString(msg.message)
lidar_endtoend_analyzer.put_pb('perception', perception)
if msg.topic == "/apollo/prediction":
if ((not is_auto_drive) and (not all_data)) or is_simulation or \
plot_planning_path or plot_planning_refpath:
continue
prediction = prediction_obstacle_pb2.PredictionObstacles()
prediction.ParseFromString(msg.message)
lidar_endtoend_analyzer.put_pb('prediction', prediction)
def callback_planning(self, data):
"""
New Planning Trajectory
"""
entity = planning_pb2.ADCTrajectory()
entity.CopyFrom(data)
basetime = entity.header.timestamp_sec
numpoints = len(entity.adc_trajectory_point)
if numpoints == 0:
print entity
return
pointx = numpy.zeros(numpoints)
pointy = numpy.zeros(numpoints)
pointspeed = numpy.zeros(numpoints)
pointtime = numpy.zeros(numpoints)
pointtheta = numpy.zeros(numpoints)
pointcur = numpy.zeros(numpoints)
pointacc = numpy.zeros(numpoints)
for idx in range(numpoints):
pointx[idx] = entity.adc_trajectory_point[idx].x
pointy[idx] = entity.adc_trajectory_point[idx].y
pointspeed[idx] = entity.adc_trajectory_point[idx].speed
pointtheta[idx] = entity.adc_trajectory_point[idx].theta
pointcur[idx] = entity.adc_trajectory_point[idx].curvature
pointacc[idx] = entity.adc_trajectory_point[idx].acceleration_s
pointtime[idx] = entity.adc_trajectory_point[
idx].relative_time + basetime
st_available = False
debug = entity.debug.debug_message
for item in debug:
if item.id == 0:
stgraph = json.loads(item.info)["st_graph_info"]
boundaries = stgraph["boundaries"]
unit_t = stgraph["unit_t"]
graph_total_t = stgraph["graph_total_t"]
graph_total_s = stgraph["graph_total_s"]
st_points = stgraph["st_points"]
st_a = [point["a"] for point in st_points]
st_v = [point["v"] for point in st_points]
st_s = [point["point"]["s"] for point in st_points]
st_t = [point["point"]["t"] for point in st_points]
st_available = True
with self.lock:
self.ax1.new_planning(pointtime, pointx, pointy)
self.ax2.new_planning(pointtime, pointspeed)
if self.ax3.title == "Curvature":
self.ax3.new_planning(pointtime, pointcur)
elif st_available:
self.ax3.new_planning(st_t, st_s, graph_total_t, graph_total_s)
if self.ax4.title == "Heading":
self.ax4.new_planning(pointtime, pointtheta)
else:
self.ax4.new_planning(pointtime, pointacc)
def plot_planning(ax, planning_file):
try:
fhandle = file(planning_file, 'r')
except:
print "Failed to open file %s" % (planning_file)
return
planning_pb = planning_pb2.ADCTrajectory()
text_format.Merge(fhandle.read(), planning_pb)
x = [p.path_point.x for p in planning_pb.trajectory_point]
y = [p.path_point.y for p in planning_pb.trajectory_point]
z = [p.v for p in planning_pb.trajectory_point]
ax.plot(x, y, z, label=planning_file)
ax.legend()
def plot_planning(ax, planning_file):
with open(planning_file, 'r') as fp:
planning_pb = planning_pb2.ADCTrajectory()
text_format.Merge(fp.read(), planning_pb)
trajectory = get_3d_trajectory(planning_pb)
ax.plot(trajectory[0],
trajectory[1],
trajectory[2],
label="Trajectory:%s" % planning_file)
paths = get_debug_paths(planning_pb)
if paths:
for name, path in paths:
ax.plot(path[0],
path[1],
label="%s:%s" % (name, planning_file))
ax.legend()
def plot_planning(ax, planning_file):
try:
fhandle = file(planning_file, 'r')
except:
print "Failed to open file %s" % (planning_file)
return
planning_pb = planning_pb2.ADCTrajectory()
text_format.Merge(fhandle.read(), planning_pb)
trajectory = get_3d_trajectory(planning_pb)
ax.plot(trajectory[0],
trajectory[1],
trajectory[2],
label="Trajectory:%s" % planning_file)
paths = get_debug_paths(planning_pb)
for name, path in paths:
ax.plot(path[0], path[1], label="%s:%s" % (name, planning_file))
ax.legend()
def generate_message(topic, filename):
"""generate message from file"""
message = None
if topic == "/apollo/planning":
message = planning_pb2.ADCTrajectory()
elif topic == "/apollo/localization/pose":
message = localization_pb2.LocalizationEstimate()
elif topic == "/apollo/perception/obstacles":
message = perception_obstacle_pb2.PerceptionObstacles()
elif topic == "/apollo/prediction":
message = prediction_obstacle_pb2.PredictionObstacles()
elif topic == "/apollo/routing_response":
message = routing_pb2.RoutingResponse()
if not message:
print "Unknown topic:", topic
sys.exit(0)
if not os.path.exists(filename):
return None
f_handle = file(filename, 'r')
text_format.Merge(f_handle.read(), message)
f_handle.close()
return message
def read(self, topics):
reader = RecordReader(self.record_file)
for msg in reader.read_messages():
if msg.topic not in topics:
continue
if msg.topic == "/apollo/canbus/chassis":
chassis = chassis_pb2.Chassis()
chassis.ParseFromString(msg.message)
data = {"chassis": chassis}
yield data
if msg.topic == "/apollo/localization/pose":
location_est = localization_pb2.LocalizationEstimate()
location_est.ParseFromString(msg.message)
data = {"pose": location_est}
yield data
if msg.topic == "/apollo/planning":
planning = planning_pb2.ADCTrajectory()
planning.ParseFromString(msg.message)
data = {"planning": planning}
yield data
def publish_trajectory(self):
planning_msg = planning_pb2.ADCTrajectory()
planning_msg.header.timestamp_sec = cyber_time.Time.now().to_sec()
planning_msg.header.module_name = "planning"
planning_msg.header.sequence_num = self.planning_msg_count
self.planning_msg_count += 1
last_point_id = self.trajectory.points_num - 1
self.nearest_point_id = self.find_nearest_point_on_trajectory()
planning_msg.total_path_length = self.trajectory[last_point_id].l - \
self.trajectory[self.nearest_point_id].l
planning_msg.total_path_time = self.trajectory[last_point_id].t - \
self.trajectory[self.nearest_point_id].t
planning_msg.gear = 1
planning_msg.engage_advice.advice = drive_state_pb2.EngageAdvice.READY_TO_ENGAGE
planning_msg.estop.is_estop = self.estop
for point in self.trajectory[self.nearest_point_id:last_point_id + 1]:
adc_point = pnc_point_pb2.TrajectoryPoint()
adc_point.path_point.x = point.x
adc_point.path_point.y = point.y
adc_point.path_point.z = point.z
adc_point.v = point.v
adc_point.a = point.a
adc_point.path_point.kappa = point.curvature
adc_point.path_point.dkappa = 0.0 # TODO! (does it matter for control?)
adc_point.path_point.theta = point.yaw
adc_point.path_point.s = point.l
adc_point.relative_time = point.t - self.trajectory[
self.nearest_point_id].t
# adc_point.relative_time = time_diff - (now - self.starttime)
planning_msg.trajectory_point.extend([adc_point])
self.cyber_trajectory_pub.write(planning_msg)
def read_planning_pb(planning_pb_file):
planning_pb = planning_pb2.ADCTrajectory()
f_handle = open(planning_pb_file, 'r')
text_format.Merge(f_handle.read(), planning_pb)
f_handle.close()
return planning_pb
axarr[1, 1].get_shared_x_axes().join(axarr[0, 0], axarr[1, 1])
controlinfo = ControlInfo(axarr)
if args.bag:
file_path = args.bag
# bag = rosbag.Bag(file_path)
reader = RecordReader(file_path)
for msg in reader.read_messages():
print(msg.timestamp, msg.topic)
if msg.topic == "/apollo/localization/pose":
localization = localization_pb2.LocalizationEstimate()
localization.ParseFromString(msg.message)
controlinfo.callback_localization(localization)
elif msg.topic == "/apollo/planning":
adc_trajectory = planning_pb2.ADCTrajectory()
adc_trajectory.ParseFromString(msg.message)
controlinfo.callback_planning(adc_trajectory)
elif msg.topic == "/apollo/control":
control_cmd = control_cmd_pb2.ControlCommand()
control_cmd.ParseFromString(msg.message)
controlinfo.callback_control(control_cmd)
elif msg.topic == "/apollo/canbus/chassis":
chassis = chassis_pb2.Chassis()
chassis.ParseFromString(msg.message)
controlinfo.callback_canbus(chassis)
print("Done reading the file")
else:
cyber.init()
# rospy.init_node('control_info', anonymous=True)
def publish_planningmsg(self):
"""
Generate New Path
"""
if not self.localization_received:
self.logger.warning(
"locaization not received yet when publish_planningmsg")
return
planningdata = planning_pb2.ADCTrajectory()
now = rospy.get_time()
planningdata.header.timestamp_sec = now
planningdata.header.module_name = "planning"
planningdata.header.sequence_num = self.sequence_num
self.sequence_num = self.sequence_num + 1
self.logger.debug(
"publish_planningmsg: before adjust start: self.start = %s, self.end=%s"
% (self.start, self.end))
if self.replan or self.sequence_num <= 1 or not self.automode:
self.logger.info(
"trigger replan: self.replan=%s, self.sequence_num=%s, self.automode=%s"
% (self.replan, self.sequence_num, self.automode))
self.restart()
else:
time_elapsed = now - self.starttime
time_diff = self.data['time'][self.start] - \
self.data['time'][self.closestpoint]
while time_diff < time_elapsed and self.start < (len(self.data) -
1):
self.start = self.start + 1
time_diff = self.data['time'][self.start] - \
self.data['time'][self.closestpoint]
xdiff_sqr = (self.data['x'][self.start] - self.carx)**2
ydiff_sqr = (self.data['y'][self.start] - self.cary)**2
if xdiff_sqr + ydiff_sqr > 4.0:
self.logger.info("trigger replan: distance larger than 2.0")
self.restart()
if self.completepath:
self.start = 0
self.end = len(self.data) - 1
else:
self.start = max(self.start - 100, 0)
self.end = min(self.start + 1000, len(self.data) - 1)
self.logger.debug(
"publish_planningmsg: after adjust start: self.start = %s, self.end=%s"
% (self.start, self.end))
for i in range(self.start, self.end):
adc_point = planning_pb2.ADCTrajectoryPoint()
adc_point.x = self.data['x'][i]
adc_point.y = self.data['y'][i]
adc_point.z = self.data['z'][i]
adc_point.speed = self.data['speed'][i] * self.speedmultiplier
adc_point.acceleration_s = self.data['acceleration'][
i] * self.speedmultiplier
adc_point.curvature = self.data['curvature'][i]
adc_point.curvature_change_rate = self.data[
'curvature_change_rate'][i]
time_diff = self.data['time'][i] - \
self.data['time'][self.closestpoint]
adc_point.relative_time = time_diff / self.speedmultiplier - (
now - self.starttime)
adc_point.theta = self.data['theta'][i]
adc_point.accumulated_s = self.data['s'][i]
planningdata.adc_trajectory_point.extend([adc_point])
planningdata.estop.is_estop = self.estop
planningdata.total_path_length = self.data['s'][self.end] - \
self.data['s'][self.start]
planningdata.total_path_time = self.data['time'][self.end] - \
self.data['time'][self.start]
planningdata.gear = int(self.data['gear'][self.start])
self.planning_pub.publish(planningdata)
self.logger.debug("Generated Planning Sequence: " +
str(self.sequence_num - 1))
def publish_planningmsg(self):
"""
Generate New Path
"""
if not self.localization_received:
self.logger.warning(
"locaization not received yet when publish_planningmsg")
return
planningdata = planning_pb2.ADCTrajectory()
now = time.time()
planningdata.header.timestamp_sec = now
planningdata.header.module_name = "planning"
planningdata.header.sequence_num = self.sequence_num
self.sequence_num = self.sequence_num + 1
self.logger.debug(
"publish_planningmsg: before adjust start: self.start = %s, self.end=%s"
% (self.start, self.end))
if self.replan or self.sequence_num <= 1 or not self.automode:
self.logger.info(
"trigger replan: self.replan=%s, self.sequence_num=%s, self.automode=%s"
% (self.replan, self.sequence_num, self.automode))
self.restart()
else:
timepoint = self.closest_time()
distpoint = self.closest_dist()
self.start = max(min(timepoint, distpoint) - 100, 0)
self.end = min(max(timepoint, distpoint) + 900, len(self.data) - 1)
xdiff_sqr = (self.data['x'][timepoint] - self.carx)**2
ydiff_sqr = (self.data['y'][timepoint] - self.cary)**2
if xdiff_sqr + ydiff_sqr > 4.0:
self.logger.info("trigger replan: distance larger than 2.0")
self.restart()
if self.completepath:
self.start = 0
self.end = len(self.data) - 1
self.logger.debug(
"publish_planningmsg: after adjust start: self.start = %s, self.end=%s"
% (self.start, self.end))
planningdata.total_path_length = self.data['s'][self.end] - \
self.data['s'][self.start]
planningdata.total_path_time = self.data['time'][self.end] - \
self.data['time'][self.start]
planningdata.gear = int(self.data['gear'][self.closest_time()])
planningdata.engage_advice.advice = \
drive_state_pb2.EngageAdvice.READY_TO_ENGAGE
for i in range(self.start, self.end):
adc_point = pnc_point_pb2.TrajectoryPoint()
adc_point.path_point.x = self.data['x'][i]
adc_point.path_point.y = self.data['y'][i]
adc_point.path_point.z = self.data['z'][i]
adc_point.v = self.data['speed'][i] * self.speedmultiplier
adc_point.a = self.data['acceleration'][i] * self.speedmultiplier
adc_point.path_point.kappa = self.data['curvature'][i]
adc_point.path_point.dkappa = self.data['curvature_change_rate'][i]
adc_point.path_point.theta = self.data['theta'][i]
if planningdata.gear == chassis_pb2.Chassis.GEAR_REVERSE:
adc_point.v = -adc_point.v
# adc_point.a = -adc_point.a
# adc_point.path_point.kappa = -adc_point.path_point.kappa
# adc_point.path_point.dkappa = -adc_point.path_point.dkappa
# adc_point.path_point.theta = math.fmod(
# self.data['theta'][i] + 2.0 * math.pi, 2.0 * math.pi)
# if adc_point.path_point.theta < 0.0:
# adc_point.path_point.theta = adc_point.path_point.theta + 2.0 * math.pi
# adc_point.path_point.theta = adc_point.path_point.theta - math.pi
time_diff = self.data['time'][i] - \
self.data['time'][self.closestpoint]
adc_point.relative_time = time_diff / self.speedmultiplier - (
now - self.starttime)
adc_point.path_point.s = self.data['s'][i]
planningdata.trajectory_point.extend([adc_point])
planningdata.estop.is_estop = self.estop
self.planning_pub.write(planningdata)
self.logger.debug("Generated Planning Sequence: " +
str(self.sequence_num - 1))
def publish_planningmsg(self):
"""
Generate New Path
"""
if not self.localization_received:
self.logger.warning(
"localization not received yet when publish_planningmsg")
return
planningdata = planning_pb2.ADCTrajectory()
now = cyber_time.Time.now().to_sec()
planningdata.header.timestamp_sec = now
planningdata.header.module_name = "planning"
planningdata.header.sequence_num = self.sequence_num
self.sequence_num = self.sequence_num + 1
self.logger.debug(
"publish_planningmsg: before adjust start: self.start=%s, self.end=%s"
% (self.start, self.end))
if self.replan or self.sequence_num <= 1 or not self.automode:
self.logger.info(
"trigger replan: self.replan=%s, self.sequence_num=%s, self.automode=%s"
% (self.replan, self.sequence_num, self.automode))
self.restart()
else:
timepoint = self.closest_time()
distpoint = self.closest_dist()
if self.data['gear'][timepoint] == self.data['gear'][distpoint]:
self.start = max(min(timepoint, distpoint), 0)
elif self.data['gear'][timepoint] == self.chassis.gear_location:
self.start = timepoint
else:
self.start = distpoint
self.logger.debug("timepoint:[%s]" % timepoint)
self.logger.debug("distpoint:[%s]" % distpoint)
self.logger.debug("trajectory start point: [%s], gear is [%s]" %
(self.start, self.data['gear'][self.start]))
self.end = self.next_gear_switch_time(self.start, len(self.data))
self.logger.debug("len of data: ", len(self.data))
self.logger.debug("trajectory end point: [%s], gear is [%s]" %
(self.end, self.data['gear'][self.end]))
xdiff_sqr = (self.data['x'][timepoint] - self.carx)**2
ydiff_sqr = (self.data['y'][timepoint] - self.cary)**2
if xdiff_sqr + ydiff_sqr > 4.0:
self.logger.info("trigger replan: distance larger than 2.0")
self.restart()
if self.completepath:
self.start = 0
self.end = len(self.data) - 1
self.logger.debug(
"publish_planningmsg: after adjust start: self.start=%s, self.end=%s"
% (self.start, self.end))
planningdata.total_path_length = self.data['s'][self.end] - \
self.data['s'][self.start]
self.logger.info("total number of planning data point: %d" %
(self.end - self.start))
planningdata.total_path_time = self.data['time'][self.end] - \
self.data['time'][self.start]
planningdata.gear = int(self.data['gear'][self.closest_time()])
planningdata.engage_advice.advice = \
drive_state_pb2.EngageAdvice.READY_TO_ENGAGE
for i in range(self.start, self.end):
adc_point = pnc_point_pb2.TrajectoryPoint()
adc_point.path_point.x = self.data['x'][i]
adc_point.path_point.y = self.data['y'][i]
adc_point.path_point.z = self.data['z'][i]
adc_point.v = self.data['speed'][i] * self.speedmultiplier
adc_point.a = self.data['acceleration'][i] * self.speedmultiplier
adc_point.path_point.kappa = self.data['curvature'][i]
adc_point.path_point.dkappa = self.data['curvature_change_rate'][i]
adc_point.path_point.theta = self.data['theta'][i]
adc_point.path_point.s = self.data['s'][i]
if CHANGE_TO_COM:
# translation vector length(length / 2 - back edge to center)
adc_point.path_point.x = adc_point.path_point.x + \
(self.vehicle_param.length // 2 - self.vehicle_param.back_edge_to_center) * \
math.cos(adc_point.path_point.theta)
adc_point.path_point.y = adc_point.path_point.y + \
(self.vehicle_param.length // 2 - self.vehicle_param.back_edge_to_center) * \
math.sin(adc_point.path_point.theta)
if planningdata.gear == chassis_pb2.Chassis.GEAR_REVERSE:
adc_point.v = -adc_point.v
adc_point.path_point.s = -adc_point.path_point.s
time_diff = self.data['time'][i] - \
self.data['time'][self.closestpoint]
adc_point.relative_time = time_diff / self.speedmultiplier - (
now - self.starttime)
planningdata.trajectory_point.extend([adc_point])
planningdata.estop.is_estop = self.estop
self.planning_pub.write(planningdata)
self.logger.debug("Generated Planning Sequence: " +
str(self.sequence_num - 1))
else:
return points_t[1]
if points_t[len(points_t) / 2] > current_t:
return find_closest_t(points_t[0:len(points_t) / 2], current_t)
elif points_t[len(points_t) / 2] < current_t:
return find_closest_t(points_t[len(points_t) / 2 + 1:], current_t)
else:
return current_t
if len(sys.argv) < 2:
print "usage: python main.py planning.pb.txt localization.pb.txt"
sys.exit()
planning_pb_file = sys.argv[1]
planning_pb = planning_pb2.ADCTrajectory()
f_handle = open(planning_pb_file, 'r')
text_format.Merge(f_handle.read(), planning_pb)
f_handle.close()
#print planning_pb_data
localization_pb_file = sys.argv[2]
localization_pb = localization_pb2.LocalizationEstimate()
f_handle = open(localization_pb_file, 'r')
text_format.Merge(f_handle.read(), localization_pb)
f_handle.close()
# plot traj
points_x = []
points_y = []
points_t = []
