def smallest_rectangle(p):
# mp.dps = 10
ch = convex_hull(p)
print(ch)
# Find extreme points
x_min_index = 0
y_max_index = uppermost_point_index(ch)
x_max_index = rightmost_point_index(ch)
y_min_index = lowermost_point_index(ch)
# Create vertical and horizontal Rays
ray_1 = Ray(ch[x_min_index], angle=pi / 2)
ray_2 = Ray(ch[y_min_index], angle=0)
ray_3 = Ray(ch[x_max_index], angle=pi / 2)
ray_4 = Ray(ch[y_max_index], angle=0)
min_rectangle = Polygon((0, 0), (99999999, 0), (99999999, 99999999),
(0, 99999999))
rotated_angle = 0
while rotated_angle <= pi / 2:
# Convert Rays to lines
line_1 = Line(ray_1)
line_2 = Line(ray_2)
line_3 = Line(ray_3)
line_4 = Line(ray_4)
# Find the intersections of the lines
p1 = line_1.intersection(line_2)[0].evalf()
p2 = line_2.intersection(line_3)[0].evalf()
p3 = line_3.intersection(line_4)[0].evalf()
p4 = line_4.intersection(line_1)[0].evalf()
current_rectangle = Polygon(p1, p2, p3, p4)
# Calculate the area of the rectangle
try:
if current_rectangle.area < min_rectangle.area:
min_rectangle = current_rectangle
except AttributeError:
min_rectangle = current_rectangle
# Find the next Ray on the convex hull for each Ray in counter clockwise direction
next_ray_1 = Ray(ch[x_min_index], ch[get_prev_index(ch, x_min_index)])
next_ray_2 = Ray(ch[y_min_index], ch[get_prev_index(ch, y_min_index)])
next_ray_3 = Ray(ch[x_max_index], ch[get_prev_index(ch, x_max_index)])
next_ray_4 = Ray(ch[y_max_index], ch[get_prev_index(ch, y_max_index)])
#print(ray_1)
#print(next_ray_1)
# Find the minimal angle to rotate each Ray until one aligns with the convex hull
angle_1 = float(positive_angle(next_ray_1.closing_angle(ray_1)))
angle_2 = float(positive_angle(next_ray_2.closing_angle(ray_2)))
angle_3 = float(positive_angle(next_ray_3.closing_angle(ray_3)))
angle_4 = float(positive_angle(next_ray_4.closing_angle(ray_4)))
#print(angle_1, angle_2, angle_3, angle_4)
min_angle = min(angle_1, angle_2, angle_3, angle_4)
# Rotate all Rays around its origin with the angle calculated
"""ray_1 = ray_1.rotate(min_angle)
ray_2 = ray_2.rotate(min_angle)
ray_3 = ray_3.rotate(min_angle)
ray_4 = ray_4.rotate(min_angle)"""
ray_1 = Ray(ray_1.p1, angle=atan(ray_1.slope) + min_angle)
ray_2 = Ray(ray_2.p1, angle=atan(ray_2.slope) + min_angle)
ray_3 = Ray(ray_3.p1, angle=atan(ray_3.slope) + min_angle)
ray_4 = Ray(ray_4.p1, angle=atan(ray_4.slope) + min_angle)
# Set the next point in the convex hull as the origin of the Ray that alligned
# and decrement the index
slope_1 = next_ray_1.slope
slope_2 = next_ray_2.slope
slope_3 = next_ray_3.slope
slope_4 = next_ray_4.slope
if min_angle == angle_1:
if slope_1 == oo:
slope_1 = pi / 2
ray_1 = Ray(next_ray_1.p2, angle=atan(slope_1))
x_min_index = get_prev_index(ch, x_min_index)
elif min_angle == angle_2:
if slope_2 == oo:
slope_2 = pi / 2
ray_2 = Ray(next_ray_2.p2, angle=atan(slope_2))
y_min_index = get_prev_index(ch, y_min_index)
elif min_angle == angle_3:
if slope_3 == oo:
slope_3 = pi / 2
ray_3 = Ray(next_ray_3.p2, angle=atan(slope_3))
x_max_index = get_prev_index(ch, x_max_index)
elif min_angle == angle_4:
if slope_4 == oo:
slope_4 = pi / 2
ray_4 = Ray(next_ray_4.p2, angle=atan(slope_4))
y_max_index = get_prev_index(ch, y_max_index)
else:
print("fail")
rotated_angle += min_angle
return min_rectangle
def repeat():
global waypoint_pointer
global in_pivot_mode
stopping_flag = False
msg = package_msg(0, 0, 0, 0)
#get latest available data
bot_x, bot_y, bot_front_x, bot_front_y = get_db_data()
#deal with goal changes
goal_x, goal_y, wp_count, loop_on = get_goal(waypoint_pointer)
Bot_Loc = Point2D(bot_x, bot_y)
Goal_Loc = Point2D(goal_x, goal_y)
#distance between goal and present loc
eucledian_dist_error = (Bot_Loc.distance(Goal_Loc)).evalf()
#angle to move so that vehicle heading is correct
r1 = Ray((bot_x, bot_y), (goal_x, goal_y))
r2 = Ray((bot_x, bot_y), (bot_front_x, bot_front_y))
angle_error = math.degrees(r1.closing_angle(r2).evalf())
#redundant check, do not remove.
if angle_error < -180:
angle_error = angle_error + 360
if angle_error > 180:
angle_error = angle_error - 360
#####################################UNCOMMENT THESE FOR DEBUGGING######################################
# print("angle error")
# print(angle_error)
# print("distance error")
# print(eucledian_dist_error)
# print("Steer PID")
# print(BOT_1.Steer)
# print("Throt PID")
# print(BOT_1.Thr)
# print("LEFT WHEEL")
# print(out_left_wheel)
# print("RIGHT WHEEL")
# print(out_right_wheel)
# print("Goal")
# print(get_goal(waypoint_pointer))
#######################################################################################################
# pivot if angle_error is too big
#TO-DO: change hard coded values into parameter's. Maybe a class that can be accessed from command line
if abs(angle_error) > 60 or in_pivot_mode:
in_pivot_mode = True
if abs(angle_error) >= 35:
pivot_speed = int(
np.interp(abs(angle_error), [35, 180], [400, 500]))
print("pivoting")
if angle_error > 0:
#pivot anti-cws
msg = package_msg(0, pivot_speed, 1, pivot_speed)
else:
#pivot cws
msg = package_msg(1, pivot_speed, 0, pivot_speed)
else:
in_pivot_mode = False
if in_pivot_mode == False:
#what to do when approaching goal, i.e run precision controller
if (eucledian_dist_error <= 100 and eucledian_dist_error > 40):
left_direction = 1
left_speed = 600 + 3 * angle_error
right_direction = 1
right_speed = 600 - 3 * angle_error
msg = package_msg(1, left_speed, 1, right_speed)
elif (eucledian_dist_error <= 40):
if stopping_flag == False:
waypoint_pointer = waypoint_pointer + 1
if waypoint_pointer == wp_count:
if loop_on:
waypoint_pointer = 0
else:
msg = package_msg(0, 0, 0, 0)
else:
#stop
msg = package_msg(0, 0, 0, 0)
#convert PID value into motor driver understandable value
#xyz_direction = 1 for forward
else:
#calculate PID values based on error obtained
BOT_1.Steer = BOT_1.pid_steering_controller(angle_error)
BOT_1.Thr = BOT_1.pid_throttle_controller(-eucledian_dist_error)
#get computed values from PID
out_left_wheel = BOT_1.Output_left_wheel()
out_right_wheel = BOT_1.Output_right_wheel()
if (out_left_wheel >= 0):
left_direction = 1
left_speed = out_left_wheel
else:
left_direction = 0
left_speed = -out_left_wheel
if (out_right_wheel >= 0):
right_direction = 1
right_speed = out_right_wheel
else:
right_direction = 0
right_speed = -out_right_wheel
msg = package_msg(left_direction, left_speed, right_direction,
right_speed)
#pack the message and send
sock.sendto(msg.encode(), (udp_host, udp_port))
#repeat every x seconds
threading.Timer(0.3, repeat).start()