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()