def follow_walls(angle_max_range, left_circle, right_circle, barrier,
delta_time):
global last_speed
prediction_distance = parameters.corner_cutting + \
parameters.straight_smoothing * last_speed
predicted_car_position = Point(0, prediction_distance)
# target_direction = Point(10 * -np.sin(angle_max_range), 10 * np.cos(angle_max_range))
# left_point = left_circle.get_closest_point(predicted_car_position)
# right_point = right_circle.get_closest_point(predicted_car_position)
error = (predicted_car_position.y * - np.tan(angle_max_range)) / \
prediction_distance
if math.isnan(error) or math.isinf(error):
error = 0
steering_angle = pid.update_and_get_correction(error, delta_time)
radius = min(left_circle.radius, right_circle.radius)
speed_limit_radius = map(parameters.radius_lower, parameters.radius_upper,
0, 1, radius) # nopep8
speed_limit_error = max(
0, 1 + parameters.steering_slow_down_dead_zone -
abs(error) * parameters.steering_slow_down) # nopep8
speed_limit_acceleration = last_speed + parameters.max_acceleration * delta_time
speed_limit_barrier = map(parameters.barrier_lower_limit,
parameters.barrier_upper_limit, 0, 1,
barrier)**parameters.barrier_exponent # nopep8
relative_speed = min(speed_limit_error, speed_limit_radius,
speed_limit_acceleration, speed_limit_barrier)
last_speed = relative_speed
speed = map(0, 1, parameters.min_throttle, parameters.max_throttle,
relative_speed) # nopep8
steering_angle = steering_angle * map(
parameters.high_speed_steering_limit_dead_zone, 1, 1,
parameters.high_speed_steering_limit, relative_speed) # nopep8
steering_angle_limit = 0.4
if abs(steering_angle) > steering_angle_limit:
steering_angle = steering_angle * steering_angle_limit / abs(
steering_angle)
# print('speed_before:',speed)
speed = speed * np.cos(abs(steering_angle / (steering_angle_limit * 1.1)))
drive(steering_angle, speed)
# print('steering_angle:', steering_angle)
# print('speed:', speed)
# show_line_in_rviz(2, [left_point, right_point],
# color=ColorRGBA(1, 1, 1, 0.3), line_width=0.005)
show_line_in_rviz(2, [Point(0, 0), predicted_car_position],
color=ColorRGBA(1, 1, 1, 0.3),
line_width=0.005)
def follow_walls(left_circle, right_circle, barrier, delta_time):
global last_speed
prediction_distance = parameters.corner_cutting + \
parameters.straight_smoothing * last_speed
predicted_car_position = Point(0, prediction_distance)
left_point = left_circle.get_closest_point(predicted_car_position)
right_point = right_circle.get_closest_point(predicted_car_position)
target_position = Point((left_point.x + right_point.x) / 2,
(left_point.y + right_point.y) / 2)
error = (target_position.x - predicted_car_position.x) / \
prediction_distance
if math.isnan(error) or math.isinf(error):
error = 0
steering_angle = pid.update_and_get_correction(error, delta_time)
radius = min(left_circle.radius, right_circle.radius)
speed_limit_radius = map(parameters.radius_lower, parameters.radius_upper,
0, 1, radius) # nopep8
speed_limit_error = max(
0, 1 + parameters.steering_slow_down_dead_zone -
abs(error) * parameters.steering_slow_down) # nopep8
speed_limit_acceleration = last_speed + parameters.max_acceleration * delta_time
speed_limit_barrier = map(parameters.barrier_lower_limit,
parameters.barrier_upper_limit, 0, 1,
barrier)**parameters.barrier_exponent # nopep8
relative_speed = min(speed_limit_error, speed_limit_radius,
speed_limit_acceleration, speed_limit_barrier)
last_speed = relative_speed
speed = map(0, 1, parameters.min_throttle, parameters.max_throttle,
relative_speed) # nopep8
steering_angle = steering_angle * map(
parameters.high_speed_steering_limit_dead_zone, 1, 1,
parameters.high_speed_steering_limit, relative_speed) # nopep8
print('steering:', steering_angle)
print('speed:', speed)
drive(steering_angle, speed)
show_line_in_rviz(2, [left_point, right_point],
color=ColorRGBA(1, 1, 1, 0.3),
line_width=0.005)
show_line_in_rviz(3, [Point(0, 0), predicted_car_position],
color=ColorRGBA(1, 1, 1, 0.3),
line_width=0.005)
show_line_in_rviz(4, [predicted_car_position, target_position],
color=ColorRGBA(1, 0.4, 0, 1))
show_line_in_rviz(
5, [Point(-2, barrier), Point(2, barrier)],
color=ColorRGBA(1, 1, 0, 1))
def greedy_exploration(points, ranges, usable_range_angle):
length = len(ranges)
bodysizeb = 0.18 + 0.5 # width of car + extra safe margin
# search by gates
safe_margin = 0.8
gate_angle = 5 # degrees, search every * degrees
number_of_detections = usable_range_angle // gate_angle - 1
detection = [[0] for k in range(number_of_detections)
] # laser points in each gate
for i in range(
number_of_detections
): # only consider the conditions: usable_range_angle <=180 degrees
gate_rad = math.radians(-usable_range_angle / 2 + (i + 1) * gate_angle)
# points in each gate
points_x_plus_y_side = points[:, 1] + points[:, 0] / np.tan(
gate_rad) # y + x/-tan(-theta)
points_x_plus_y_bottom = points[:, 1] - points[:, 0] * np.tan(gate_rad)
if gate_rad < 0:
for j in range(length):
if 0.5 * bodysizeb/-np.sin(gate_rad) > points_x_plus_y_side[j] > 0.5 * bodysizeb/np.sin(gate_rad)\
and points_x_plus_y_bottom[j] > 0:
if ranges[j] < safe_margin:
detection[i] = [0]
break
else:
detection[i].append(ranges[j])
elif gate_rad > 0:
for j in range(length):
if 0.5 * bodysizeb/-np.sin(gate_rad) < points_x_plus_y_side[j] < 0.5 * bodysizeb/np.sin(gate_rad)\
and points_x_plus_y_bottom[j] > 0:
if ranges[j] < safe_margin:
detection[i] = [0]
break
else:
detection[i].append(ranges[j])
else: # gate_rad == 0
for j in range(length):
if 0.1 > points[j, 0] > -0.1:
if ranges[j] < safe_margin:
detection[i] = [0]
break
else:
detection[i].append(ranges[j])
detection[i] = mean(detection[i])
max_gate = np.argmax(detection)
angle_max_range = math.radians(-usable_range_angle / 2 +
(max_gate + 1) * gate_angle)
max_range = 10 # only use to visualize the target direction, doesn't matter what value it is.
p11 = Point(0.5 * bodysizeb / -np.cos(angle_max_range), 0)
p12 = Point(
max_range * -np.sin(angle_max_range) +
0.5 * bodysizeb / -np.cos(angle_max_range),
max_range * np.cos(angle_max_range))
p21 = Point(0.5 * bodysizeb / np.cos(angle_max_range), 0)
p22 = Point(
max_range * -np.sin(angle_max_range) +
0.5 * bodysizeb / np.cos(angle_max_range),
max_range * np.cos(angle_max_range))
show_line_in_rviz(4, [p11, p12],
color=ColorRGBA(1, 0.4, 0, 1),
line_width=0.005)
show_line_in_rviz(5, [p21, p22],
color=ColorRGBA(1, 0.4, 0, 1),
line_width=0.005)
show_line_in_rviz(6, [Point(0, 0), Point(0, safe_margin)],
color=ColorRGBA(0.5, 0.4, 0.7, 1),
line_width=0.02)
return angle_max_range