def process_segment(plan, xL, yL, thL, last_ddy, free_space, path):
x = plan[:, 0]
# calculate params
zeros = tf.zeros_like(last_ddy)
p = params([zeros, zeros, zeros, last_ddy], tf.unstack(plan, axis=1))
# calculate xy coords of segment
x_glob, y_glob, th_glob, curvature = _calculate_global_xyth_and_curvature(
p, x, xL, yL, thL)
# calcualte length of segment
length, segments = _calculate_length(x_glob, y_glob)
# calculate violations
curvature_violation = tf.reduce_sum(
tf.nn.relu(tf.abs(curvature[:, 1:]) - Car.max_curvature) * segments,
-1)
curvature_sum = tf.reduce_sum(tf.abs(curvature), -1)
invalid, supervised_loss = invalidate(x_glob, y_glob, th_glob, free_space,
path)
return x_glob, y_glob, th_glob, curvature_violation, invalid, length, x_glob[:,
-1], y_glob[:,
-1], th_glob[:,
-1], curvature_sum, supervised_loss
def calculate_next_point(plan, xL, yL, thL, last_ddy):
x = plan[:, 0]
# calculate params
zeros = tf.zeros_like(last_ddy)
p = params([zeros, zeros, zeros, last_ddy], tf.unstack(plan, axis=1))
# calculate xy coords of segment
x_glob, y_glob, th_glob = _calculate_global_xyth(p, x, xL, yL, thL)
return x_glob[:, -1], y_glob[:, -1], th_glob[:, -1]