def process_image(self, image, video=False): draw_image = np.copy(image) image = self._normalize(image) likely_windows = self._find_likely_windows(image) if video: likely_windows = combine_boxes(likely_windows, image.shape) results, self.window.score = average_boxes(likely_windows, self.window.score, image.shape) draw_image = draw_boxes(draw_image, likely_windows, color=(255, 0, 0), thick=6) bounding_boxes = draw_boxes(draw_image, results, color=(0, 0, 255), thick=3) if video: return bounding_boxes else: return bounding_boxes, likely_windows
def main(): create_directories(IMAGES_OUTPUT_DIR) working_zone_polygon = Polygon(WORKING_ZONE_POLY_POINTS) working_zone_points_cv = np.array(WORKING_ZONE_POLY_POINTS, np.int32).reshape((-1, 1, 2)) counter = 1 print("Connecting to smoothie") smoothie = create_smoothie_connection(config.SMOOTHIE_HOST) print("Loading neural network") detector = detection.YoloOpenCVDetection() print("Loading camera") with adapters.CameraAdapterIMX219_170() as camera: time.sleep(2) print("Moving camera to the view position") # go to the view position (y min, x max / 2) smoothie.custom_move_to(config.XY_F_MAX, X=config.X_MAX / 2, Y=config.Y_MIN) smoothie.wait_for_all_actions_done() print("Starting main loop, running in continuous movement mode") # main control loop while True: scan_move_continuously(smoothie, camera, detector, working_zone_polygon, CONTINUOUS_TRAVEL_FORCE, CONTINUOUS_TRAVEL_DISTANCE_MM * ONE_MM_IN_SMOOTHIE) print("Starting plants extraction in step mode") # switch to the step mode while True: time.sleep(DELAY_BEFORE_TAKING_FRAME) frame = camera.get_image() plant_boxes = detector.detect(frame) # save photo if SAVE_IMAGES: img_y_c, img_x_c = int(frame.shape[0] / 2), int(frame.shape[1] / 2) frame = draw_zone_circle(frame, img_x_c, img_y_c, UNDISTORTED_ZONE_RADIUS) frame = draw_zone_poly(frame, working_zone_points_cv) frame = detection.draw_boxes(frame, plant_boxes) save_image(IMAGES_OUTPUT_DIR, frame, counter, "View") counter += 1 if len(plant_boxes) == 0: # start moving continuously if there's no plants left print("No plants detected - switching to continuous mode") break extract_all_plants(smoothie, camera, detector, working_zone_polygon, frame, plant_boxes) # go to the view position (y min, x max / 2) print("Moving camera to the view position") smoothie.custom_move_to(config.XY_F_MAX, X=config.X_MAX / 2, Y=config.Y_MIN) smoothie.wait_for_all_actions_done() print("Moving step forward") move_forward(STEP_TRAVEL_FORCE, STEP_DISTANCE_MM * ONE_MM_IN_SMOOTHIE, smoothie) # F1100, B-5.2 = 30 cm with max speed (B-104 F1900 for min speed 30 cm) smoothie.wait_for_all_actions_done()
def debug_save_image(img_output_dir, label, frame, plants_boxes, undistorted_zone_radius, poly_zone_points_cv): # debug image saving if config.SAVE_DEBUG_IMAGES: img_y_c, img_x_c = int(frame.shape[0] / 2), int(frame.shape[1] / 2) frame = draw_zone_circle(frame, img_x_c, img_y_c, undistorted_zone_radius) frame = draw_zone_poly(frame, poly_zone_points_cv) frame = detection.draw_boxes(frame, plants_boxes) save_image(img_output_dir, frame, None, label)
def draw_bounding_box(self, image): image = self.process._normalize(image) likely_windows = self.process._find_likely_windows(image) likely_windows = combine_boxes(likely_windows, image.shape) results, self.process.window.score = average_boxes( likely_windows, self.process.window.score, image.shape) draw_image = draw_boxes(image, likely_windows, color=(255, 0, 0), thick=6) return draw_image
def main(): create_directories(IMAGES_OUTPUT_DIR) working_zone_polygon = Polygon(WORKING_ZONE_POLY_POINTS) working_zone_points_cv = np.array(WORKING_ZONE_POLY_POINTS, np.int32).reshape((-1, 1, 2)) counter = 1 print("Connecting to smoothie") smoothie = create_smoothie_connection(config.SMOOTHIE_HOST) print("Loading neural network") detector = detection.YoloOpenCVDetection() print("Loading camera") with adapters.CameraAdapterIMX219_170() as camera: time.sleep(2) print("Moving camera to the view position") # go to the view position (y min, x max / 2) smoothie.custom_move_to(config.XY_F_MAX, X=config.X_MAX / 2, Y=config.Y_MIN) smoothie.wait_for_all_actions_done() print("Starting main loop") # main control loop while True: for _ in range(0, SQUARE_SIDE_MM, STEP_DISTANCE_MM): time.sleep(DELAY_BEFORE_TAKING_FRAME) frame = camera.get_image() plant_boxes = detector.detect(frame) # save photo if SAVE_IMAGES: img_y_c, img_x_c = int(frame.shape[0] / 2), int(frame.shape[1] / 2) frame = draw_zone_circle(frame, img_x_c, img_y_c, UNDISTORTED_ZONE_RADIUS) frame = draw_zone_poly(frame, working_zone_points_cv) frame = detection.draw_boxes(frame, plant_boxes) save_image(IMAGES_OUTPUT_DIR, frame, counter, "View") counter += 1 if len(plant_boxes) == 0: print("No plants detected") else: print("Detected", len(plant_boxes), "plants") extract_all_plants(smoothie, camera, detector, working_zone_polygon, frame, plant_boxes) print("Moving camera to the view position") # go to the view position (y min, x max / 2) smoothie.custom_move_to(config.XY_F_MAX, X=config.X_MAX / 2, Y=config.Y_MIN) smoothie.wait_for_all_actions_done() print("Moving step forward") move_forward(STEP_TRAVEL_FORCE, STEP_DISTANCE_MM * ONE_MM_IN_SMOOTHIE, smoothie) # F1100, B-5.2 = 30 cm with max speed (B-104 F1900 for min speed 30 cm) smoothie.wait_for_all_actions_done() # turn for 90 degrees after edge is passed turn_90_degrees(1000, -35, 1100, -75, smoothie)
def debug_save_image(img_output_dir, label, frame, plants_boxes, undistorted_zone_radius, poly_zone_points_cv): # TODO: data gathering temporary hardcoded if ALLOW_GATHERING: save_image(DATA_GATHERING_DIR, frame, None, label) # debug image saving if config.SAVE_DEBUG_IMAGES: frame = draw_zone_circle(frame, config.SCENE_CENTER_X, config.SCENE_CENTER_Y, undistorted_zone_radius) frame = draw_zone_poly(frame, poly_zone_points_cv) frame = detection.draw_boxes(frame, plants_boxes) save_image(img_output_dir, frame, None, label)
def encodeFrame(): global thread_lock while True: # Acquire thread_lock to access the global video_frame object with thread_lock: global video_frame, use_detector if video_frame is None: continue original_frame = video_frame[config.CROP_H_FROM:config.CROP_H_TO, config.CROP_W_FROM:config.CROP_W_TO] if use_detector: global detector plants_boxes = detector.detect(original_frame) if draw_zones: undistorted_zone_radius = config.UNDISTORTED_ZONE_RADIUS poly_zone_points_cv = np.array(config.WORKING_ZONE_POLY_POINTS, np.int32).reshape((-1, 1, 2)) original_frame = draw_zone_circle(original_frame, config.SCENE_CENTER_X, config.SCENE_CENTER_Y, undistorted_zone_radius) frame = draw_zone_poly(original_frame, poly_zone_points_cv) else: frame = original_frame if draw_control_point: points_map = config.IMAGE_CONTROL_POINTS_MAP for i in range(len(points_map)): frame = draw_zone_circle(frame, points_map[i][0],points_map[i][1], 5) if use_detector: frameFinal = detection.draw_boxes(frame, plants_boxes) else: frameFinal = frame if use_save_detector: global out out.write(frameFinal) return_key, encoded_image = cv2.imencode(".jpg", frameFinal) if not return_key: continue # Output image as a byte array yield(b'--frame\r\n' b'Content-Type: image/jpeg\r\n\r\n' + bytearray(encoded_image) + b'\r\n')
def draw_data_in_frame(frame, undistorted_zone_radius=None, poly_zone_points_cv=None, pdz_cv_rect=None, plants_boxes=None): if undistorted_zone_radius is not None: frame = ImageSaver.draw_zone_circle(frame, config.SCENE_CENTER_X, config.SCENE_CENTER_Y, undistorted_zone_radius, (255, 40, 162)) frame = ImageSaver.draw_zone_circle(frame, config.SCENE_CENTER_X, config.SCENE_CENTER_Y, 5, (255, 40, 162), -1) if poly_zone_points_cv is not None: frame = ImageSaver.draw_zone_poly(frame, poly_zone_points_cv, (255, 0, 0)) if plants_boxes is not None: frame = detection.draw_boxes(frame, plants_boxes) if pdz_cv_rect is not None: frame = cv.rectangle(frame, pdz_cv_rect[0], pdz_cv_rect[1], (16, 127, 237), 3) return frame
center = [(top[0] + h[0] // 2, left[0] + w[0] // 2), (top[1] + h[1] // 2, left[1] + w[1] // 2)] euc_dist = np.sqrt((center[1][0] - center[0][0])**2 + (center[1][1] - center[0][1])**2) inv_dist = euc_dist / (sum(h) / 2) prod = inv_dist / p if prod < 1.11: violators.add(i) violators.add(j) return [persons[i] for i in violators] def get_processed_img(detector, img): persons = detector.get_persons(img) violators = get_violators(persons) img = draw_boxes(img, violators) return img if __name__ == '__main__': img = cv2.imread('images/street.png') detector = default_dedector() persons = detector.get_persons(img) violators = get_violators(persons) img = draw_boxes(img, violators) cv2.imshow('boxes', img) cv2.waitKey()
print("min={}, max={}".format(np.min(image), np.max(image))) img_boxes, heatmap = detection.find_cars(image, y_start, y_stop, scale, svc, X_scaler, orient, pix_per_cell, cell_per_block, spatial_size, hist_bins) if thresh: heatmap = detection.apply_threshold(heatmap, 2) if label: # Find final boxes from heatmap using label function labels = measurement.label(heatmap) draw_img = detection.draw_labeled_bboxes(np.copy(image), labels) else: draw_img = detection.draw_boxes(np.copy(image), img_boxes) f, (ax1, ax2) = plt.subplots(1, 2, figsize=(14, 7)) ax1.imshow(draw_img) ax1.set_title("Car Positions", fontsize=24) ax2.imshow(heatmap, cmap="hot") ax2.set_title("Heat Map", fontsize=24) if show: plt.show() if save: save_file_name = "heatmap_{}{}{}".format( "thresh_" if thresh else "", "label_" if label else "", os.path.basename(image_file.replace(".jpg", ".png"))) save_location = "./output_images/{}".format(save_file_name) f.savefig(save_location, bbox_inches="tight")
def get_processed_img(detector, img): persons = detector.get_persons(img) violators = get_violators(persons) img = draw_boxes(img, violators) return img
def extract_all_plants(smoothie: adapters.SmoothieAdapter, camera: adapters.CameraAdapterIMX219_170, precise_det: detection.YoloOpenCVDetection, working_zone_polygon: Polygon, frame, plant_boxes: list, undistorted_zone_radius, working_zone_points_cv, img_output_dir): """Extract all plants found in current position""" img_y_c, img_x_c = int(frame.shape[0] / 2), int(frame.shape[1] / 2) # loop over all detected plants for box in plant_boxes: # go to the extraction position Y min smoothie.custom_move_to(config.XY_F_MAX, X=config.X_MAX / 2 / config.XY_COEFFICIENT_TO_MM, Y=config.Y_MIN) smoothie.wait_for_all_actions_done() box_x, box_y = box.get_center_points() # if plant is in working zone (can be reached by cork) if is_point_in_poly(box_x, box_y, working_zone_polygon): # extraction loop while True: box_x, box_y = box.get_center_points() # if plant inside undistorted zone if is_point_in_circle(box_x, box_y, img_x_c, img_y_c, config.UNDISTORTED_ZONE_RADIUS): print("Plant is in undistorted zone") # calculate values to move camera over a plant sm_x = px_to_smoothie_value(box_x, img_x_c, config.ONE_MM_IN_PX) sm_y = -px_to_smoothie_value(box_y, img_y_c, config.ONE_MM_IN_PX) # swap camera and cork for extraction immediately sm_x += config.CORK_TO_CAMERA_DISTANCE_X sm_y += config.CORK_TO_CAMERA_DISTANCE_Y # move cork over a plant res = smoothie.custom_move_for(config.XY_F_MAX, X=sm_x, Y=sm_y) smoothie.wait_for_all_actions_done() if res != smoothie.RESPONSE_OK: print( "Couldn't move cork over plant, smoothie error occurred:", res) break # extraction, cork down res = smoothie.custom_move_for( F=1700, Z=config.EXTRACTION_Z ) # TODO: calculation -Z depending on box size smoothie.wait_for_all_actions_done() if res != smoothie.RESPONSE_OK: print( "Couldn't move the extractor down, smoothie error occurred:", res) break # extraction, cork up res = smoothie.ext_cork_up() smoothie.wait_for_all_actions_done() if res != smoothie.RESPONSE_OK: msg = "Couldn't move the extractor up, smoothie error occurred: " + res + \ "emergency exit as I don't want break corkscrew." print(msg) exit(1) # Daisy additional corners extraction if box.get_name( ) == "Daisy": # TODO: need to create flexible extraction method choosing (maybe dict of functions) box_x_half, box_y_half = box.get_sizes() box_x_half, box_y_half = int(box_x_half / 2 / config.ONE_MM_IN_PX), \ int(box_y_half / 2 / config.ONE_MM_IN_PX) for x_shift, y_shift in [[-box_x_half, box_y_half], [0, -box_y_half * 2], [box_x_half * 2, 0], [0, box_y_half * 2]]: # move to the corner response = smoothie.custom_move_for( config.XY_F_MAX, X=x_shift, Y=y_shift) smoothie.wait_for_all_actions_done() if response != smoothie.RESPONSE_OK: msg = "Aborting movement to the corner (couldn't reach): " + response print(msg) break # extraction, cork down res = smoothie.custom_move_for( F=1700, Z=config.EXTRACTION_Z ) # TODO: calculation -Z depending on box size smoothie.wait_for_all_actions_done() if res != smoothie.RESPONSE_OK: msg = "Couldn't move the extractor down, smoothie error occurred: " + res print(msg) break # extraction, cork up res = smoothie.ext_cork_up() smoothie.wait_for_all_actions_done() if res != smoothie.RESPONSE_OK: msg = "Couldn't move the extractor up, smoothie error occurred: " + res + \ "emergency exit as I don't want break corkscrew." print(msg) exit(1) break # if outside undistorted zone but in working zone else: # calculate values for move camera closer to a plant control_point = get_closest_control_point( box_x, box_y, config.IMAGE_CONTROL_POINTS_MAP) sm_x, sm_y = control_point[2], control_point[3] # move camera closer to a plant res = smoothie.custom_move_for(config.XY_F_MAX, X=sm_x, Y=sm_y) smoothie.wait_for_all_actions_done() if res != smoothie.RESPONSE_OK: print( "Couldn't move to plant, smoothie error occurred:", res) break # make new photo and re-detect plants frame = camera.get_image() temp_plant_boxes = precise_det.detect(frame) # check case if no plants detected if len(temp_plant_boxes) == 0: print( "No plants detected (plant was in working zone before), trying to move on next item" ) break # debug image saving if config.SAVE_DEBUG_IMAGES: frame = draw_zone_circle(frame, img_x_c, img_y_c, undistorted_zone_radius) frame = draw_zone_poly(frame, working_zone_points_cv) frame = detection.draw_boxes(frame, temp_plant_boxes) save_image(img_output_dir, frame, None, "(extraction specify)") # get closest box (update current box from main list coordinates after moving closer) box = min_plant_box_dist(temp_plant_boxes, img_x_c, img_y_c) # if not in working zone else: print("Skipped", str(box), "(not in working area)") # set camera back to the Y min smoothie.custom_move_to(config.XY_F_MAX, X=config.X_MAX / 2 / config.XY_COEFFICIENT_TO_MM, Y=config.Y_MIN) smoothie.wait_for_all_actions_done()
def move_to_point_and_extract( coords_from_to: list, gps: adapters.GPSUbloxAdapter, vesc_engine: adapters.VescAdapter, smoothie: adapters.SmoothieAdapter, camera: adapters.CameraAdapterIMX219_170, periphery_det: detection.YoloOpenCVDetection, precise_det: detection.YoloOpenCVDetection, client, logger_full: utility.Logger, logger_table: utility.Logger, report_field_names, used_points_history: list, nav: navigation.GPSComputing, working_zone_polygon, undistorted_zone_radius, working_zone_points_cv, img_output_dir): """ Moves to the given target point and extracts all weeds on the way. :param coords_from_to: :param gps: :param vesc_engine: :param smoothie: :param camera: :param periphery_det: :param precise_det: :param client: :param logger_full: :param logger_table: :param report_field_names: :param used_points_history: :param nav: :param working_zone_polygon: :param undistorted_zone_radius: :param working_zone_points_cv: :param img_output_dir: :return: """ raw_angles_history = [] stop_helping_point = nav.get_coordinate(coords_from_to[1], coords_from_to[0], 90, 1000) prev_maneuver_time = time.time() prev_point = gps.get_last_position() vesc_engine.apply_rpm(int(config.VESC_RPM_SLOW)) vesc_engine.start_moving() # main navigation control loop while True: frame = camera.get_image() plants_boxes = periphery_det.detect(frame) # debug image saving if config.SAVE_DEBUG_IMAGES: img_y_c, img_x_c = int(frame.shape[0] / 2), int(frame.shape[1] / 2) frame = draw_zone_circle(frame, img_x_c, img_y_c, undistorted_zone_radius) frame = draw_zone_poly(frame, working_zone_points_cv) frame = detection.draw_boxes(frame, plants_boxes) save_image(img_output_dir, frame, None, "(view scan)") # stop and extract all plants if there any in working zone if len(plants_boxes) > 0 and any_plant_in_working_zone( plants_boxes, working_zone_polygon): vesc_engine.stop_moving( ) # TODO: robot is not stopping instantly (maybe add here some engines backward force and sleep time for camera?) # rescan after we stopped frame = camera.get_image() plants_boxes = periphery_det.detect(frame) # debug image saving if config.SAVE_DEBUG_IMAGES: img_y_c, img_x_c = int(frame.shape[0] / 2), int( frame.shape[1] / 2) frame = draw_zone_circle(frame, img_x_c, img_y_c, undistorted_zone_radius) frame = draw_zone_poly(frame, working_zone_points_cv) frame = detection.draw_boxes(frame, plants_boxes) save_image(img_output_dir, frame, None, "(view scan after vesc stop)") # do extractions if there still any plants in working zone if len(plants_boxes) > 0 and any_plant_in_working_zone( plants_boxes, working_zone_polygon): extract_all_plants(smoothie, camera, precise_det, working_zone_polygon, frame, plants_boxes, undistorted_zone_radius, working_zone_points_cv, img_output_dir) vesc_engine.start_moving() # navigation control cur_pos = gps.get_last_position() if str(cur_pos) == str(prev_point): # msg = "Got the same position, added to history, calculations skipped. Am I stuck?" # print(msg) # logger_full.write(msg + "\n") continue used_points_history.append(cur_pos.copy()) if not client.sendData("{};{}".format(cur_pos[0], cur_pos[1])): msg = "[Client] Connection closed !" print(msg) logger_full.write(msg + "\n") distance = nav.get_distance(cur_pos, coords_from_to[1]) msg = "Distance to B: " + str(distance) # print(msg) logger_full.write(msg + "\n") # check if arrived _, side = nav.get_deviation(coords_from_to[1], stop_helping_point, cur_pos) # if distance <= config.COURSE_DESTINATION_DIFF: # old way if side != 1: # TODO: maybe should use both side and distance checking methods at once vesc_engine.stop_moving() # msg = "Arrived (allowed destination distance difference " + str(config.COURSE_DESTINATION_DIFF) + " mm)" msg = "Arrived to " + str(coords_from_to[1]) # print(msg) logger_full.write(msg + "\n") break # reduce speed if near the target point if config.USE_SPEED_LIMIT: distance_from_start = nav.get_distance(coords_from_to[0], cur_pos) if distance < config.DECREASE_SPEED_TRESHOLD or distance_from_start < config.DECREASE_SPEED_TRESHOLD: vesc_engine.apply_rpm(int(config.VESC_RPM_SLOW)) else: vesc_engine.apply_rpm(config.VESC_RPM_FAST) # do maneuvers not more often than specified value cur_time = time.time() if cur_time - prev_maneuver_time < config.MANEUVERS_FREQUENCY: continue prev_maneuver_time = cur_time msg = "Timestamp: " + str(cur_time) # print(msg) logger_full.write(msg + "\n") msg = "Prev: " + str(prev_point) + " Cur: " + str(cur_pos) + " A: " + str(coords_from_to[0]) \ + " B: " + str(coords_from_to[1]) # print(msg) logger_full.write(msg + "\n") raw_angle = nav.get_angle(prev_point, cur_pos, cur_pos, coords_from_to[1]) # sum(e) if len(raw_angles_history) >= config.WINDOW: raw_angles_history.pop(0) raw_angles_history.append(raw_angle) sum_angles = sum(raw_angles_history) if sum_angles > config.SUM_ANGLES_HISTORY_MAX: msg = "Sum angles " + str(sum_angles) + " is bigger than max allowed value " + \ str(config.SUM_ANGLES_HISTORY_MAX) + ", setting to " + str(config.SUM_ANGLES_HISTORY_MAX) # print(msg) logger_full.write(msg + "\n") sum_angles = config.SUM_ANGLES_HISTORY_MAX elif sum_angles < -config.SUM_ANGLES_HISTORY_MAX: msg = "Sum angles " + str(sum_angles) + " is less than min allowed value " + \ str(-config.SUM_ANGLES_HISTORY_MAX) + ", setting to " + str(-config.SUM_ANGLES_HISTORY_MAX) # print(msg) logger_full.write(msg + "\n") sum_angles = -config.SUM_ANGLES_HISTORY_MAX angle_kp_ki = raw_angle * config.KP + sum_angles * config.KI target_angle_sm = angle_kp_ki * -config.A_ONE_DEGREE_IN_SMOOTHIE # smoothie -Value == left, Value == right ad_wheels_pos = smoothie.get_adapter_current_coordinates()["A"] sm_wheels_pos = smoothie.get_smoothie_current_coordinates()["A"] # compute order angle (smoothie can't turn for huge values immediately also as cancel movement, # so we need to do nav. actions in steps) order_angle_sm = target_angle_sm - ad_wheels_pos # check for out of update frequency and smoothie execution speed range (for nav wheels) if order_angle_sm > config.MANEUVERS_FREQUENCY * config.A_DEGREES_PER_SECOND * \ config.A_ONE_DEGREE_IN_SMOOTHIE: msg = "Order angle changed from " + str( order_angle_sm) + " to " + str( config.MANEUVERS_FREQUENCY * config.A_DEGREES_PER_SECOND + config.A_ONE_DEGREE_IN_SMOOTHIE ) + " due to exceeding degrees per tick allowed range." # print(msg) logger_full.write(msg + "\n") order_angle_sm = config.MANEUVERS_FREQUENCY * config.A_DEGREES_PER_SECOND * \ config.A_ONE_DEGREE_IN_SMOOTHIE elif order_angle_sm < -(config.MANEUVERS_FREQUENCY * config.A_DEGREES_PER_SECOND * config.A_ONE_DEGREE_IN_SMOOTHIE): msg = "Order angle changed from " + str( order_angle_sm) + " to " + str(-( config.MANEUVERS_FREQUENCY * config.A_DEGREES_PER_SECOND * config.A_ONE_DEGREE_IN_SMOOTHIE )) + " due to exceeding degrees per tick allowed range." # print(msg) logger_full.write(msg + "\n") order_angle_sm = -(config.MANEUVERS_FREQUENCY * config.A_DEGREES_PER_SECOND * config.A_ONE_DEGREE_IN_SMOOTHIE) # convert to global smoothie coordinates order_angle_sm += ad_wheels_pos # checking for out of smoothie supported range if order_angle_sm > config.A_MAX: msg = "Global order angle changed from " + str(order_angle_sm) + " to config.A_MAX = " + \ str(config.A_MAX) + " due to exceeding smoothie allowed values range." # print(msg) logger_full.write(msg + "\n") order_angle_sm = config.A_MAX elif order_angle_sm < config.A_MIN: msg = "Global order angle changed from " + str(order_angle_sm) + " to config.A_MIN = " + \ str(config.A_MIN) + " due to exceeding smoothie allowed values range." # print(msg) logger_full.write(msg + "\n") order_angle_sm = config.A_MIN raw_angle = round(raw_angle, 2) angle_kp_ki = round(angle_kp_ki, 2) order_angle_sm = round(order_angle_sm, 2) sum_angles = round(sum_angles, 2) distance = round(distance, 2) ad_wheels_pos = round(ad_wheels_pos, 2) sm_wheels_pos = round(sm_wheels_pos, 2) gps_quality = cur_pos[2] msg = str(gps_quality).ljust(5) + str(raw_angle).ljust(8) + str( angle_kp_ki).ljust(8) + str(order_angle_sm).ljust(8) + str( sum_angles).ljust(8) + str(distance).ljust(13) + str( ad_wheels_pos).ljust(8) + str(sm_wheels_pos).ljust(9) print(msg) logger_full.write(msg + "\n") # load sensors data to csv s = "," msg = str(gps_quality) + s + str(raw_angle) + s + str(angle_kp_ki) + s + str(order_angle_sm) + s + \ str(sum_angles) + s + str(distance) + s + str(ad_wheels_pos) + s + str(sm_wheels_pos) vesc_data = vesc_engine.get_sensors_data(report_field_names) if vesc_data is not None: msg += s for key in vesc_data: msg += str(vesc_data[key]) + s msg = msg[:-1] logger_table.write(msg + "\n") prev_point = cur_pos response = smoothie.nav_turn_wheels_to(order_angle_sm, config.A_F_MAX) if response != smoothie.RESPONSE_OK: msg = "Smoothie response is not ok: " + response + "\n" print(msg) logger_full.write(msg + "\n\n")
def main(): log_counter = 1 log_dir = "log/" if not os.path.exists(log_dir): try: os.mkdir(log_dir) except OSError: print("Creation of the directory %s failed" % log_dir) else: print("Successfully created the directory %s " % log_dir) smoothie = adapters.SmoothieAdapter(config.SMOOTHIE_HOST) detector = detection.YoloOpenCVDetection() smoothie.ext_align_cork_center(config.XY_F_MAX) smoothie.wait_for_all_actions_done() with adapters.CameraAdapterIMX219_170() as camera: time.sleep(5) image = camera.get_image() img_y_c, img_x_c = int(image.shape[0] / 2), int(image.shape[1] / 2) plant_boxes = detector.detect(image) plant_boxes = sort_plant_boxes_dist(plant_boxes, config.CORK_CENTER_X, config.CORK_CENTER_Y) # check if no plants detected if len(plant_boxes) < 1: print("No plants detected on view scan, exiting.") cv.imwrite( log_dir + str(log_counter) + " starting - see no plants.jpg", image) exit(0) # log log_img = image.copy() log_img = detection.draw_boxes(log_img, plant_boxes) log_img = draw_zones_circle(log_img, img_x_c, img_y_c, undistorted_zone_radius, working_zone_radius) cv.imwrite(log_dir + str(log_counter) + " starting.jpg", log_img) log_counter += 1 for box in plant_boxes: smoothie.ext_align_cork_center( config.XY_F_MAX) # camera in real center smoothie.wait_for_all_actions_done() box_x, box_y = box.get_center_points() # if inside the working zone if is_point_in_circle(box_x, box_y, img_x_c, img_y_c, working_zone_radius): while True: box_x, box_y = box.get_center_points() # if inside undistorted zone if is_point_in_circle(box_x, box_y, img_x_c, img_y_c, undistorted_zone_radius): # calculate values to move camera over a plant sm_x = -px_to_smoohie_value( box_x, config.CORK_CENTER_X, config.ONE_MM_IN_PX) sm_y = px_to_smoohie_value(box_y, config.CORK_CENTER_Y, config.ONE_MM_IN_PX) # move camera over a plant res = smoothie.custom_move_for(config.XY_F_MAX, X=sm_x, Y=sm_y) smoothie.wait_for_all_actions_done() if res != smoothie.RESPONSE_OK: print( "Couldn't move camera over plant, smoothie error occurred:", res) exit(1) # move cork to the camera position res = smoothie.custom_move_for(config.XY_F_MAX, Y=57) smoothie.wait_for_all_actions_done() if res != smoothie.RESPONSE_OK: print( "Couldn't move cork over plant, smoothie error occurred:", res) exit(1) # waiting confirmation for extraction (just to make people see how it's going on) input( "Ready to plant extraction, press enter to begin") # extraction, cork down res = smoothie.custom_move_for(config.Z_F_MAX, Z=-30) smoothie.wait_for_all_actions_done() if res != smoothie.RESPONSE_OK: print( "Couldn't move the extractor down, smoothie error occurred:", res) exit(1) # extraction, cork up res = smoothie.ext_cork_up() smoothie.wait_for_all_actions_done() if res != smoothie.RESPONSE_OK: print( "Couldn't move the extractor up, smoothie error occurred:", res) exit(1) break # if outside undistorted zone but in working zone else: # calculate values for move camera closer to a plant sm_x = -px_to_smoohie_value(box_x, img_x_c, config.ONE_MM_IN_PX) sm_y = px_to_smoohie_value(box_y, img_y_c, config.ONE_MM_IN_PX) # move for a half distance, dist is not < 10 sm_x = int(sm_x / 2) if sm_x / 2 > 10 else 10 sm_y = int(sm_y / 2) if sm_y / 2 > 10 else 10 # move camera closer to a plant res = smoothie.custom_move_for(config.XY_F_MAX, X=sm_x, Y=sm_y) smoothie.wait_for_all_actions_done() if res != smoothie.RESPONSE_OK: print( "Couldn't move to plant, smoothie error occurred:", res) exit(1) # make new photo and re-detect plants temp_img = camera.get_image() temp_plant_boxes = detector.detect(temp_img) # check if no plants detected if len(temp_plant_boxes) < 1: print( "No plants detected (plant was in undistorted zone before), trying to move on next item" ) temp_img = draw_zones_circle( temp_img, img_x_c, img_y_c, undistorted_zone_radius, working_zone_radius) cv.imwrite( log_dir + str(log_counter) + " in undistorted branch - see no plants.jpg", temp_img) log_counter += 1 break # log log_img = temp_img.copy() log_img = detection.draw_boxes(log_img, temp_plant_boxes) log_img = draw_zones_circle(log_img, img_x_c, img_y_c, undistorted_zone_radius, working_zone_radius) cv.imwrite( log_dir + str(log_counter) + " in undistorted branch - all.jpg", log_img) # get closest box (exactly update current box from main list coordinates after moving closer) box = min_plant_box_dist(temp_plant_boxes, img_x_c, img_y_c) # log log_img = temp_img.copy() log_img = detection.draw_box(log_img, box) log_img = draw_zones_circle(log_img, img_x_c, img_y_c, undistorted_zone_radius, working_zone_radius) cv.imwrite( log_dir + str(log_counter) + " in undistorted branch - closest.jpg", log_img) log_counter += 1 # if not in working zone else: print("skipped", str(box), "(not in working area)") print("Script executing is done.")
def main(): time.sleep(5) log_counter = 1 if not os.path.exists(LOG_DIR): try: os.mkdir(LOG_DIR) except OSError: print("Creation of the directory %s failed" % LOG_DIR) logging.error("Creation of the directory %s failed" % LOG_DIR) else: print("Successfully created the directory %s " % LOG_DIR) logging.info("Successfully created the directory %s " % LOG_DIR) # working zone pre-calculations # these points list is changed for usage in matplotlib (it has differences in the coords system) # working_zone_points_plt = list( # map(lambda item: [item[0], config.CROP_H_TO - config.CROP_H_FROM - item[1]], WORKING_ZONE_POLY_POINTS)) working_zone_polygon = Polygon(WORKING_ZONE_POLY_POINTS) # these points array is used for drawing zone using OpenCV working_zone_points_cv = np.array(WORKING_ZONE_POLY_POINTS, np.int32).reshape((-1, 1, 2)) # remove old images from log dir print("Removing .jpg images from log directory") logging.debug("Removing .jpg images from log directory") clear_log_dir() # create smoothieboard adapter (API for access and control smoothieboard) while True: try: smoothie = adapters.SmoothieAdapter(config.SMOOTHIE_HOST) print("Successfully connected to smoothie") logging.info("Successfully connected to smoothie") break except OSError as error: logging.warning(repr(error)) print(repr(error)) detector = detection.YoloOpenCVDetection() with adapters.CameraAdapterIMX219_170() as camera: print("Warming up the camera") logging.debug("Warming up the camera") time.sleep(5) # main loop, detection and motion while True: logging.debug("Starting detection and motion main loop iteration") # go to scan position # smoothie.ext_align_cork_center(config.XY_F_MAX) smoothie.custom_move_to(config.XY_F_MAX, X=config.X_MAX / 2, Y=config.Y_MIN) smoothie.wait_for_all_actions_done() image = camera.get_image() img_y_c, img_x_c = int(image.shape[0] / 2), int(image.shape[1] / 2) plant_boxes = detector.detect(image) plant_boxes = sort_plant_boxes_dist(plant_boxes, config.X_MAX / 2, config.Y_MIN) # check if no plants detected if len(plant_boxes) < 1: print("No plants detected on view scan (img " + str(log_counter) + "), moving forward") logging.info("No plants detected on view scan (img " + str(log_counter) + "), moving forward") log_img = image.copy() log_img = draw_zones(log_img, img_x_c, img_y_c, UNDISTORTED_ZONE_RADIUS, working_zone_points_cv) if config.SAVE_DEBUG_IMAGES: cv.imwrite( LOG_DIR + str(log_counter) + " overview scan (see no plants).jpg", log_img) log_counter += 1 # move forward for 30 sm res = smoothie.custom_move_for(1000, B=5.43) smoothie.wait_for_all_actions_done() if res != smoothie.RESPONSE_OK: print( "Couldn't move forward (for 30 sm), smoothie error occurred:", res) logging.critical( "Couldn't move forward (for 30 sm), smoothie error occurred: " + res) # exit(1) continue else: print("Found " + str(len(plant_boxes)) + " plants on view scan (img " + str(log_counter) + ")") logging.info("Found " + str(len(plant_boxes)) + " plants on view scan (img " + str(log_counter) + ")") log_img = image.copy() log_img = draw_zones(log_img, img_x_c, img_y_c, UNDISTORTED_ZONE_RADIUS, working_zone_points_cv) log_img = detection.draw_boxes(log_img, plant_boxes) if config.SAVE_DEBUG_IMAGES: cv.imwrite( LOG_DIR + str(log_counter) + " overview scan (see " + str(len(plant_boxes)) + " plants).jpg", log_img) log_counter += 1 # loop over all detected plants for box in plant_boxes: logging.debug("Starting loop over plants list iteration") # smoothie.ext_align_cork_center(config.XY_F_MAX) # camera in real center smoothie.custom_move_to(config.XY_F_MAX, X=config.X_MAX / 2, Y=config.Y_MIN) smoothie.wait_for_all_actions_done() box_x, box_y = box.get_center_points() """ print("Processing plant on x=" + str(box_x) + " y=" + str(box_y)) logging.info("Processing plant on x=" + str(box_x) + " y=" + str(box_y)) """ # if plant is in working zone and can be reached by cork if is_point_in_poly(box_x, box_y, working_zone_polygon): print("Plant is in working zone") logging.info("Plant is in working zone") while True: print("Starting extraction loop") logging.info("Starting extraction loop") box_x, box_y = box.get_center_points() print("Processing plant in x=" + str(box_x) + " y=" + str(box_y)) logging.info("Processing plant in x=" + str(box_x) + " y=" + str(box_y)) # if inside undistorted zone if is_point_in_circle(box_x, box_y, img_x_c, img_y_c, UNDISTORTED_ZONE_RADIUS): print("Plant is in undistorted zone") logging.info("Plant is in undistorted zone") # calculate values to move camera over a plant sm_x = px_to_smoothie_value( box_x, img_x_c, config.ONE_MM_IN_PX) sm_y = -px_to_smoothie_value( box_y, img_y_c, config.ONE_MM_IN_PX) # swap camera and cork for extraction immediately sm_y += CORK_CAMERA_DISTANCE print("Calculated smoothie moving coordinates X=" + str(sm_x) + " Y=" + str(sm_y)) logging.debug( "Calculated smoothie moving coordinates X=" + str(sm_x) + " Y=" + str(sm_y)) ad_cur_coord = smoothie.get_adapter_current_coordinates( ) print("Adapter coordinates: " + str(ad_cur_coord)) logging.info("Adapter coordinates: " + str(ad_cur_coord)) sm_cur_coord = smoothie.get_smoothie_current_coordinates( ) print("Smoothie coordinates: " + str(sm_cur_coord)) logging.info("Smoothie coordinates: " + str(sm_cur_coord)) # move cork over a plant print("Moving cork to the plant") logging.info("Moving cork to the plant") res = smoothie.custom_move_for(config.XY_F_MAX, X=sm_x, Y=sm_y) smoothie.wait_for_all_actions_done() if res != smoothie.RESPONSE_OK: print( "Couldn't move cork over plant, smoothie error occurred:", res) logging.critical( "Couldn't move cork over plant, smoothie error occurred: " + str(res)) # exit(1) # temp debug 1 log_img = camera.get_image() if config.SAVE_DEBUG_IMAGES: cv.imwrite( LOG_DIR + str(log_counter) + " extracting (cork in upper position).jpg", log_img) log_counter += 1 # extraction, cork down print("Extracting plant (cork down)") logging.info("Extracting plant (cork down)") res = smoothie.custom_move_for(config.Z_F_MAX, Z=-30) smoothie.wait_for_all_actions_done() if res != smoothie.RESPONSE_OK: print( "Couldn't move the extractor down, smoothie error occurred:", res) logging.critical( "Couldn't move the extractor down, smoothie error occurred:" + str(res)) # exit(1) # temp debug 2 log_img = camera.get_image() if config.SAVE_DEBUG_IMAGES: cv.imwrite( LOG_DIR + str(log_counter) + " extracting (cork in lower position).jpg", log_img) log_counter += 1 # extraction, cork up print("Extracting plant (cork up)") logging.info("Extracting plant (cork up)") res = smoothie.ext_cork_up() smoothie.wait_for_all_actions_done() if res != smoothie.RESPONSE_OK: print( "Couldn't move the extractor up, smoothie error occurred:", res) logging.critical( "Couldn't move the extractor up, smoothie error occurred:" + str(res)) # exit(1) break # if outside undistorted zone but in working zone else: print( "Plant is outside undistorted zone, moving to") logging.info( "Plant is outside undistorted zone, moving to") # calculate values for move camera closer to a plant control_point = get_closest_control_point( box_x, box_y, IMAGE_CONTROL_POINTS_MAP) sm_x = control_point[2] sm_y = control_point[3] debug_text = "Moving to px x=" + str(control_point[0]) + " y=" + str(control_point[1]) + \ " (control point #" + str(control_point[4]) + ")" print(debug_text) logging.info(debug_text) # move camera closer to a plant res = smoothie.custom_move_for(config.XY_F_MAX, X=sm_x, Y=sm_y) smoothie.wait_for_all_actions_done() if res != smoothie.RESPONSE_OK: print( "Couldn't move to plant, smoothie error occurred:", res) logging.critical( "Couldn't move to plant, smoothie error occurred: " + str(res)) # exit(1) # make new photo and re-detect plants image = camera.get_image() temp_plant_boxes = detector.detect(image) # check if no plants detected if len(temp_plant_boxes) < 1: print( "No plants detected (plant was in working zone before), trying to move on\ next item") logging.info( "No plants detected (plant was in working zone before), trying to move on\ next item") log_img = image.copy() log_img = draw_zones(log_img, img_x_c, img_y_c, UNDISTORTED_ZONE_RADIUS, working_zone_points_cv) if config.SAVE_DEBUG_IMAGES: cv.imwrite( LOG_DIR + str(log_counter) + " in working zone branch - see no plants.jpg", log_img) log_counter += 1 break # log log_img = image.copy() log_img = draw_zones(log_img, img_x_c, img_y_c, UNDISTORTED_ZONE_RADIUS, working_zone_points_cv) log_img = detection.draw_boxes( log_img, temp_plant_boxes) if config.SAVE_DEBUG_IMAGES: cv.imwrite( LOG_DIR + str(log_counter) + " in working zone branch - all plants.jpg", log_img) log_counter += 1 # get closest box (exactly update current box from main list coordinates after moving closer) box = min_plant_box_dist(temp_plant_boxes, img_x_c, img_y_c) # log log_img = image.copy() log_img = draw_zones(log_img, img_x_c, img_y_c, UNDISTORTED_ZONE_RADIUS, working_zone_points_cv) log_img = detection.draw_box(log_img, box) if config.SAVE_DEBUG_IMAGES: cv.imwrite( LOG_DIR + str(log_counter) + " in working zone branch - closest plant.jpg", log_img) log_counter += 1 # if not in working zone else: print("Skipped", str(box), "(not in working area)") logging.info("Skipped " + str(box) + " (not in working area)") # move forward for 30 sm res = smoothie.custom_move_for(1000, B=5.43) smoothie.wait_for_all_actions_done() if res != smoothie.RESPONSE_OK: print( "Couldn't move forward (for 30 sm), smoothie error occurred:", res) logging.critical( "Couldn't move forward (for 30 sm), smoothie error occurred: " + str(res))
def draw_sliding_window(self, img): windows = self.process._find_likely_windows(img) return draw_boxes(img, windows)
def main(): #meters_multiplier = ask_meters_multiplier() #distance, force = ask_speed_mode(meters_multiplier) rpm = float(input("Set RPM: ")) moving_time = float(input("Set moving time (seconds): ")) print("Initializing detector...") create_directories(WITH_PLANTS_OUTPUT_PATH, NO_PLANTS_OUTPUT_PATH, DRAWN_BOXES_OUTPUT_PATH) detector = detection.YoloOpenCVDetection() print("Initializing smoothie...") smoothie = adapters.SmoothieAdapter(config.SMOOTHIE_HOST) # move camera to the low-center position print("Moving camera to position...") smoothie.custom_move_to(config.XY_F_MAX, X=config.X_MAX / 2, Y=config.Y_MIN) smoothie.wait_for_all_actions_done() print("Initializing camera...") with CameraAdapterIMX219_170(ISP_DIGITAL_GAIN_RANGE_FROM, ISP_DIGITAL_GAIN_RANGE_TO, GAIN_RANGE_FROM, GAIN_RANGE_TO, EXPOSURE_TIME_RANGE_FROM, EXPOSURE_TIME_RANGE_TO, AE_LOCK) as camera: time.sleep(2) counter = 1 try: with adapters.VescAdapter(rpm, moving_time, VESC_ALIVE_FREQ, VESC_CHECK_FREQ, VESC_PORT, VESC_BAUDRATE) as vesc_engine: # start moving forward vesc_engine.start_moving() # get and save images until keyboard interrupt print("Starting to take photos...") while vesc_engine.is_movement_allowed(): frame = camera.get_image() plants = detector.detect(frame) # sort and save photo if len(plants) > 0: save_image(WITH_PLANTS_OUTPUT_PATH, frame, counter, "With plants") if SAVE_WITH_BOXES: detection.draw_boxes(frame, plants) save_image(DRAWN_BOXES_OUTPUT_PATH, frame, counter, "With boxes") else: save_image(NO_PLANTS_OUTPUT_PATH, frame, counter, "No plants") counter += 1 if counter % 100 == 0: print("Saved", counter, "photos") except KeyboardInterrupt: vesc_engine.stop_moving() print("Stopped by a keyboard interrupt (Ctrl + C)") print("Saved", counter, "photos") exit(0) print("Saved", counter, "photos") print("Done!")