def main(): print("Loading smoothie adapter") smoothie = adapters.SmoothieAdapter(config.SMOOTHIE_HOST) while True: print("Calibrating cork") smoothie.ext_calibrate_cork() smoothie.wait_for_all_actions_done() print("Aligning cork to center") res = smoothie.ext_align_cork_center(config.XY_F_MAX) smoothie.wait_for_all_actions_done() print(res) print("Extracting, cork down") # extraction, cork down res = smoothie.custom_move_for(F=1700, Z=-35) smoothie.wait_for_all_actions_done() print(res) # extraction, cork up print("Extracting, cork up") res = smoothie.ext_cork_up() smoothie.wait_for_all_actions_done() print(res) print("30 sec delay...") time.sleep(30)
def main(): 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) # create smoothieboard adapter (API for access and control smoothieboard) while True: try: smoothie = adapters.SmoothieAdapter(config.SMOOTHIE_HOST) log_msg = "Successfully connected to smoothie" print(log_msg) logging.info(log_msg) break except OSError as error: logging.warning(repr(error)) print(repr(error)) detector_simulation = multiprocessing.Process(target=detection_process) detector_simulation.start() log_msg = "Started working at " + str(str(datetime.datetime.now()).split(".")[:-1])[2:-2].replace(":", "-") print(log_msg) logging.info(log_msg) while True: do_extractions(smoothie) move_forward(smoothie)
def main(): try: create_directories(WITH_PLANTS_DIR, WITHOUT_PLANTS_DIR) working_zone_polygon = Polygon(config.WORKING_ZONE_POLY_POINTS) with adapters.SmoothieAdapter(config.SMOOTHIE_HOST) as smoothie: with adapters.CameraAdapterIMX219_170(config.CROP_W_FROM, config.CROP_W_TO, config.CROP_H_FROM, config.CROP_H_TO, config.CV_ROTATE_CODE, config.ISP_DIGITAL_GAIN_RANGE_FROM, config.ISP_DIGITAL_GAIN_RANGE_TO, config.GAIN_RANGE_FROM, config.GAIN_RANGE_TO, config.EXPOSURE_TIME_RANGE_FROM, config.EXPOSURE_TIME_RANGE_TO, config.AE_LOCK, config.CAMERA_W, config.CAMERA_H, config.CAMERA_W, config.CAMERA_H, config.CAMERA_FRAMERATE, config.CAMERA_FLIP_METHOD) as camera: with adapters.VescAdapter(config.VESC_RPM_SLOW, config.VESC_MOVING_TIME, config.VESC_ALIVE_FREQ, config.VESC_CHECK_FREQ, config.VESC_PORT, config.VESC_BAUDRATE) as vesc_engine: counter = 1 session_label = input("Set session label (added to names): ") input("Press enter to start.") print("Data gathering started.") while True: counter = gather_data(smoothie, camera, counter, session_label, working_zone_polygon) # move_forward_smoothie(1100, -5.2, smoothie) # F1100, B-5.2 = 30 cm with max speed (B-104 F1900 for min speed 30 cm) # for smoothie moving forward control vesc_engine.start_moving() vesc_engine.wait_for_stop() except KeyboardInterrupt: print("Stopped by a keyboard interrupt") except: print(traceback.format_exc()) finally: smoothie.disconnect() print("Data gathering done.", str(counter - 1), "images collected at this session.")
def main(): smoothie = adapters.SmoothieAdapter(config.SMOOTHIE_HOST) meters_multiplier = ask_meters_multiplier() distance, force = ask_speed_mode(meters_multiplier) moving = False # main loop while True: command = input("Hit enter to " + ("STOP" if moving else "MOVE") + ", type anything to exit: ") if command != "": if moving: print("Stopping movement...") smoothie.halt() smoothie.reset() print("Done.") break if not moving: response = smoothie.custom_move_for(force, B=distance) if response == smoothie.RESPONSE_OK: print("Moving forward for", meters_multiplier, "meters") else: print("Couldn't move forward, smoothie error occurred:", response) exit(1) else: smoothie.halt() smoothie.reset() print("Movement interrupted, standing") moving = not moving
def main(): print("Loading smoothie adapter") smoothie = adapters.SmoothieAdapter(config.SMOOTHIE_HOST) print("Aligning cork to center") res = smoothie.ext_align_cork_center(config.XY_F_MAX) smoothie.wait_for_all_actions_done() if res != smoothie.RESPONSE_OK: print("Couldn't align cork to center, smoothie error occurred:", res) exit(1) print("Extracting") # 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) print("Done!")
def create_smoothie_connection(smoothie_ip): """Create smoothieboard adapter instance (API for access and smoothieboard control)""" while True: try: smoothie = adapters.SmoothieAdapter(smoothie_ip) log_msg = "Successfully connected to smoothie" print(log_msg) return smoothie except OSError as error: print(repr(error))
def initSmoothie(logger: utility.Logger): smoothie_vesc_addr = utility.get_smoothie_vesc_addresses() if config.SMOOTHIE_BACKEND == 1: smoothie_address = config.SMOOTHIE_HOST else: if "smoothie" in smoothie_vesc_addr: smoothie_address = smoothie_vesc_addr["smoothie"] else: msg = "Couldn't get smoothie's USB address!" logger.write_and_flush(msg+"\n") print(msg) exit(1) smoothie = adapters.SmoothieAdapter(smoothie_address) return smoothie
def main(): output_dir = "gathered_data/" if not os.path.exists(output_dir): try: os.mkdir(output_dir) except OSError: print("Creation of the directory %s failed" % output_dir) else: print("Successfully created the directory %s " % output_dir) print("Loading...") sma = adapters.SmoothieAdapter(config.SMOOTHIE_HOST) if config.USE_X_AXIS_CALIBRATION and config.USE_Y_AXIS_CALIBRATION: sma.ext_align_cork_center(config.XY_F_MAX) pca = adapters.PiCameraAdapter() print("Loading complete.") dist = float(input("Move straight for (meters): ")) dist *= 1000 # convert to mm dist = int(dist) label = input("Label for that session: ") sep = " " # offset = int(input("Take image every (mm): ")) offset = 75 # 151 mm is full AOI for B in range(0, dist, offset): image = pca.get_image() cv.imwrite( output_dir + str(datetime.datetime.now()) + sep + str(B) + sep + label + ".jpg", image) sma.nav_move_forward(int(offset * config.B_ONE_MM_IN_SMOOTHIE), config.B_F_MAX) sma.wait_for_all_actions_done() # time.sleep(2) # if function above is not working properly # take last image image = pca.get_image() cv.imwrite( output_dir + str(datetime.datetime.now()) + sep + "last" + sep + label + ".jpg", image) print("Done.")
def main(): sma = adapters.SmoothieAdapter(config.SMOOTHIE_HOST) pca = adapters.PiCameraAdapter() det = detection.YoloOpenCVDetection() sma.ext_align_cork_center(config.XY_F_MAX) sma.wait_for_all_actions_done() img = pca.get_image() plant_boxes = det.detect(img) plant_boxes = sort_plant_boxes_dist(plant_boxes, config.CORK_CENTER_X, config.CORK_CENTER_Y) for box in plant_boxes: sma.ext_align_cork_center(config.XY_F_MAX) sma.wait_for_all_actions_done() box_x, box_y = box.get_center_points() # if inside the working zone if point_is_in_rect(box_x, box_y, wl, wt, wr, wb): while True: box_x, box_y = box.get_center_points() 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) res = sma.custom_move_for(config.XY_F_MAX, X=sm_x, Y=sm_y) sma.wait_for_all_actions_done() # check if no movement errors if res != sma.RESPONSE_OK: print("Smoothie error occurred:", res) exit() # if inside undistorted zone if point_is_in_rect(box_x, box_y, ul, ut, ur, ub): input("Ready to plant extraction, press enter to begin") sma.ext_do_extraction(config.Z_F_MAX) sma.wait_for_all_actions_done() break # if outside undistorted zone but in working zone else: temp_img = pca.get_image() temp_plant_boxes = det.detect(temp_img) # get closest box box = sort_plant_boxes_dist(temp_plant_boxes, config.CORK_CENTER_X, config.CORK_CENTER_Y)[0] # if not in working zone else: print(str(box), "is not in working area, switching to next")
def __init__(self): self.all_tests = 14 self.pass_tests = 0 self.smoothie = adapters.SmoothieAdapter(config.SMOOTHIE_HOST) self.vesc = adapters.VescAdapter(-2800, 10, config.VESC_ALIVE_FREQ, config.VESC_CHECK_FREQ, config.VESC_PORT, config.VESC_BAUDRATE) self.gps = adapters.GPSUbloxAdapter(config.GPS_PORT, config.GPS_BAUDRATE, config.GPS_POSITIONS_TO_KEEP) self.camera = adapters.CameraAdapterIMX219_170( config.CROP_W_FROM, config.CROP_W_TO, config.CROP_H_FROM, config.CROP_H_TO, config.CV_ROTATE_CODE, config.ISP_DIGITAL_GAIN_RANGE_FROM, config.ISP_DIGITAL_GAIN_RANGE_TO, config.GAIN_RANGE_FROM, config.GAIN_RANGE_TO, config.EXPOSURE_TIME_RANGE_FROM, config.EXPOSURE_TIME_RANGE_TO, config.AE_LOCK, config.CAMERA_W, config.CAMERA_H, config.CAMERA_W, config.CAMERA_H, config.CAMERA_FRAMERATE, config.CAMERA_FLIP_METHOD) self.name = datetime.datetime.now().strftime("%d-%m-%Y %H:%M") self.logger = utility.Logger(self.name + '.txt')
def main(): points_history = [] prev_point = None try: nav = navigation.GPSComputing() print("Initializing...", end=" ") with adapters.SmoothieAdapter(config.SMOOTHIE_HOST) as smoothie: with adapters.VescAdapter(config.VESC_RPM, config.VESC_MOVING_TIME, config.VESC_ALIVE_FREQ, config.VESC_CHECK_FREQ, config.VESC_PORT, config.VESC_BAUDRATE) as vesc_engine: with adapters.GPSUbloxAdapter( config.GPS_PORT, config.GPS_BAUDRATE, config.GPS_POSITIONS_TO_KEEP) as gps: print("done.") field_gps_coords = ask_for_ab_points(gps) save_gps_coordinates( field_gps_coords, "field " + get_current_time() + ".txt") input("Press enter to start moving") # start moving forward vesc_engine.start_moving() # main navigation control loop # TO DO: this loop is working much faster than gps, need to evaluate sleep time or waiting for new P while True: time.sleep(0.5) cur_pos = gps.get_last_position() # if str(cur_pos) == prev_point: # continue # prev_point = str(cur_pos) points_history.append(cur_pos.copy()) # check if arrived # TO DO: it won't work if deviation > course destination diff, as robot will be far away on some # side and will never get too close to the path ending point distance = nav.get_distance(cur_pos, field_gps_coords[1]) print("Distance to B:", distance) if distance <= config.COURSE_DESTINATION_DIFF: vesc_engine.stop_moving() print( "Arrived (allowed destination distance difference", config.COURSE_DESTINATION_DIFF, "mm)") break # check for course deviation. if deviation is bigger than a threshold deviation, side = nav.get_deviation( field_gps_coords[0], field_gps_coords[1], cur_pos) nav_wheels_position = smoothie.get_adapter_current_coordinates( )["A"] side_text = "(left)" if side == -1 else "(right)" if side == 1 else "(center)" print("A:", field_gps_coords[0], "B:", field_gps_coords[1], "Cur:", cur_pos) print("Deviation:", deviation, "Max dev.:", config.COURSE_SIDE_DEVIATION_MAX, "side flag:", side, side_text) if deviation > config.COURSE_SIDE_DEVIATION_MAX: # deviation to the left side if side == -1: # if not turned wheels yet # TO DO: try to make different wheels turning values for different deviation if not (-config.COURSE_ADJ_SMC_VAL - 0.001 < nav_wheels_position < -config.COURSE_ADJ_SMC_VAL + 0.001): # TO DO: check abs(COURSE_ADJ_SMC_VAL - nav_wheels_position) and set bigger or # lesser turning value print("Turning wheels left", -config.COURSE_ADJ_SMC_VAL) smoothie.nav_turn_wheels_to( -config.COURSE_ADJ_SMC_VAL, config.A_F_MAX) else: print("Wheels already at left side:", nav_wheels_position) # deviation to the right side elif side == 1: # if not turned wheels yet # TO DO: try to make different wheels turning values for different deviation if not (config.COURSE_ADJ_SMC_VAL - 0.001 < nav_wheels_position < config.COURSE_ADJ_SMC_VAL + 0.001): # TO DO: check abs(COURSE_ADJ_SMC_VAL - nav_wheels_position) and set bigger or # lesser turning value print("Turning wheels right:", config.COURSE_ADJ_SMC_VAL) smoothie.nav_turn_wheels_to( config.COURSE_ADJ_SMC_VAL, config.A_F_MAX) else: print("Wheels already at right side:", nav_wheels_position) # it's an error if deviation is big but side flag is telling us that we're on moving vector else: print("Abnormal state: deviation =", deviation, "at", side, "side. Emergency stop applied.") exit(1) # if deviation is ok (0 or less than an adj. threshold) else: # if not turned wheels yet # TO DO: try to make different wheels turning values for different deviation if not (-0.001 < nav_wheels_position < 0.001): # TO DO: check abs(COURSE_ADJ_SMC_VAL - nav_wheels_position) and set bigger or # lesser turning value print("Turning wheels to center") smoothie.nav_align_wheels_center( config.A_F_MAX) else: print("Wheels already at center:", nav_wheels_position) print() print("Done!") except KeyboardInterrupt: print("Stopped by a keyboard interrupt (Ctrl + C)") finally: save_gps_coordinates(points_history, "gps_history " + get_current_time() + ".txt")
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 main(): create_directories(config.DEBUG_IMAGES_PATH) working_zone_polygon = Polygon(config.WORKING_ZONE_POLY_POINTS) working_zone_points_cv = np.array(config.WORKING_ZONE_POLY_POINTS, np.int32).reshape((-1, 1, 2)) view_zone_polygon = Polygon(config.VIEW_ZONE_POLY_POINTS) view_zone_points_cv = np.array(config.VIEW_ZONE_POLY_POINTS, np.int32).reshape((-1, 1, 2)) nav = navigation.GPSComputing() used_points_history = [] raw_angles_history = [] logger_full = utility.Logger("full log " + get_current_time() + ".txt") logger_table = utility.Logger("table log " + get_current_time() + ".csv") # sensors picking report_field_names = [ 'temp_fet_filtered', 'temp_motor_filtered', 'avg_motor_current', 'avg_input_current', 'rpm', 'input_voltage' ] report_file = open('report.csv', 'w', newline='') report_writer = csv.DictWriter(report_file, fieldnames=report_field_names) report_writer.writeheader() # QGIS and sensor data transmitting path = os.path.abspath(os.getcwd()) sensor_processor = SensorProcessing.SensorProcessing(path, 0) sensor_processor.startServer() client = socketForRTK.Client.Client(4000) time.sleep(1) if not client.connectionToServer(): msg = "Connection refused for Server RTK." print(msg) logger_full.write(msg + "\n") sensor_processor.startSession() try: msg = "Initializing..." print(msg) logger_full.write(msg + "\n") print("Loading periphery detector...") periphery_detector = detection.YoloOpenCVDetection( config.PERIPHERY_CLASSES_FILE, config.PERIPHERY_CONFIG_FILE, config.PERIPHERY_WEIGHTS_FILE, config.PERIPHERY_INPUT_SIZE, config.PERIPHERY_CONFIDENCE_THRESHOLD, config.PERIPHERY_NMS_THRESHOLD, config.PERIPHERY_DNN_BACKEND, config.PERIPHERY_DNN_TARGET) print("Loading smoothie...") smoothie = adapters.SmoothieAdapter(config.SMOOTHIE_HOST) print("Loading vesc...") vesc_engine = adapters.VescAdapter(config.VESC_RPM_SLOW, config.VESC_MOVING_TIME, config.VESC_ALIVE_FREQ, config.VESC_CHECK_FREQ, config.VESC_PORT, config.VESC_BAUDRATE) print("Loading GPS...") gps = adapters.GPSUbloxAdapter(config.GPS_PORT, config.GPS_BAUDRATE, config.GPS_POSITIONS_TO_KEEP) print("Loading camera...") camera = adapters.CameraAdapterIMX219_170( config.CROP_W_FROM, config.CROP_W_TO, config.CROP_H_FROM, config.CROP_H_TO, config.CV_ROTATE_CODE, config.ISP_DIGITAL_GAIN_RANGE_FROM, config.ISP_DIGITAL_GAIN_RANGE_TO, config.GAIN_RANGE_FROM, config.GAIN_RANGE_TO, config.EXPOSURE_TIME_RANGE_FROM, config.EXPOSURE_TIME_RANGE_TO, config.AE_LOCK, config.CAMERA_W, config.CAMERA_H, config.CAMERA_W, config.CAMERA_H, config.CAMERA_FRAMERATE, config.CAMERA_FLIP_METHOD) # set smoothie's A axis to 0 (nav turn wheels) response = smoothie.set_current_coordinates(A=0) if response != smoothie.RESPONSE_OK: msg = "Failed to set A=0 on smoothie (turning wheels init position), response message:\n" + response print(msg) logger_full.write(msg + "\n") # load field coordinates field_gps_coords = load_coordinates( config.INPUT_GPS_FIELD_FILE) # [A, B, C, D] if len(field_gps_coords) != 4: msg = "Expected 4 gps points in " + config.INPUT_GPS_FIELD_FILE + ", got " + str( len(field_gps_coords)) print(msg) logger_full.write(msg + "\n") exit(1) # generate path points path_points = build_path(field_gps_coords, nav, logger_full) if len(path_points) > 0: save_gps_coordinates( path_points, "generated_path " + get_current_time() + ".txt") else: msg = "List of path points is empty, saving canceled." print(msg) logger_full.write(msg + "\n") if len(path_points) < 2: msg = "Expected at least 2 points in path, got " + str(len(path_points)) + \ " instead (1st point is starting point)." print(msg) logger_full.write(msg + "\n") exit(1) # ask permission to start moving msg = "Initializing done. Press enter to start moving." input(msg) logger_full.write(msg + "\n") msg = 'GpsQ|Raw ang|Res ang|Ord ang|Sum ang|Distance |Adapter|Smoothie|' print(msg) logger_full.write(msg + "\n") msg = 'GpsQ,Raw ang,Res ang,Ord ang,Sum ang,Distance,Adapter,Smoothie,' for field_name in report_field_names: msg += field_name + "," msg = msg[:-1] logger_table.write(msg + "\n") # path points visiting loop for i in range(1, len(path_points)): from_to = [path_points[i - 1], path_points[i]] from_to_dist = nav.get_distance(from_to[0], from_to[1]) msg = "Current movement vector: " + str( from_to) + " Vector size: " + str(from_to_dist) # print(msg) logger_full.write(msg + "\n") move_to_point(from_to, used_points_history, gps, vesc_engine, smoothie, logger_full, client, nav, raw_angles_history, report_writer, report_field_names, logger_table, periphery_detector, camera, working_zone_points_cv) msg = "Done!" print(msg) logger_full.write(msg + "\n") except KeyboardInterrupt: msg = "Stopped by a keyboard interrupt (Ctrl + C)" print(msg) logger_full.write(msg + "\n") except Exception: msg = "Exception occurred:\n" + traceback.format_exc() print(msg) logger_full.write(msg + "\n") finally: try: print("Stopping movement...") vesc_engine.stop_moving() except: pass try: print("Releasing camera...") camera.release() except: pass # save log data if len(used_points_history) > 0: save_gps_coordinates( used_points_history, "used_gps_history " + get_current_time() + ".txt") else: msg = "used_gps_history list has 0 elements!" print(msg) logger_full.write(msg + "\n") adapter_points_history = gps.get_last_positions_list() if len(adapter_points_history) > 0: save_gps_coordinates( adapter_points_history, "adapter_gps_history " + get_current_time() + ".txt") else: msg = "adapter_gps_history list has 0 elements!" print(msg) logger_full.write(msg + "\n") # close log and hardware connections logger_full.close() logger_table.close() report_file.close() smoothie.disconnect() vesc_engine.disconnect() gps.disconnect() # close transmitting connections sensor_processor.endSession() client.closeConnection() sensor_processor.stopServer()
def main(): nav = navigation.GPSComputing() used_points_history = [] raw_angles_history = [] logger = utility.Logger("console " + get_current_time() + ".txt") # QGIS and sensor data transmitting path = os.path.abspath(os.getcwd()) sensor_processor = SensorProcessing.SensorProcessing(path, 0) sensor_processor.startServer() client = socketForRTK.Client.Client(4000) time.sleep(1) if not client.connectionToServer(): msg = "Connection refused for Server RTK." print(msg) logger.write(msg + "\n") sensor_processor.startSession() try: msg = "Initializing..." print(msg) logger.write(msg + "\n") smoothie = adapters.SmoothieAdapter(config.SMOOTHIE_HOST) vesc_engine = adapters.VescAdapter(int(config.VESC_RPM / 2), config.VESC_MOVING_TIME, config.VESC_ALIVE_FREQ, config.VESC_CHECK_FREQ, config.VESC_PORT, config.VESC_BAUDRATE) gps = adapters.GPSUbloxAdapter(config.GPS_PORT, config.GPS_BAUDRATE, config.GPS_POSITIONS_TO_KEEP) # set smoothie's A axis to 0 (nav turn wheels) response = smoothie.set_current_coordinates(A=0) if response != smoothie.RESPONSE_OK: msg = "Failed to set A=0 on smoothie (turning wheels init position), response message:\n" + response print(msg) logger.write(msg + "\n") msg = "Initializing done." print(msg) logger.write(msg + "\n") # pick from gps field_gps_coords = load_coordinates( config.INPUT_GPS_FIELD_FILE) # [A, B, C, D] if len(field_gps_coords) != 4: msg = "Expected 4 gps points in " + config.INPUT_GPS_FIELD_FILE + ", got " + str( len(field_gps_coords)) print(msg) logger.write(msg) field_gps_coords.append(field_gps_coords[0].copy( )) # this makes robot move back to starting point (point A) field_gps_coords.append(field_gps_coords[1].copy()) # start moving forward msg = "Press enter to start moving" input(msg) logger.write(msg + "\n") # perimeter bypass loop for i in range(1, len(field_gps_coords) - 1): # current vector is marked as XY where X2 and Y1 is corner maneuver points # example: current side is AB which marked as XY, then point X2 is on AB, Y1 is on BC and # X2 and Y1 near ^ABC angle (XYZ). ABCD are "global" points which corresponding field.txt file # while XYZ displays currently processed points # XY vector current_vector = [field_gps_coords[i - 1], field_gps_coords[i]] cur_vec_dist = nav.get_distance(current_vector[0], current_vector[1]) msg = "Current movement vector: " + str( current_vector) + " Vector size: " + str(cur_vec_dist) print(msg) logger.write(msg) # check if moving vector is too small for maneuvers if config.MANEUVER_START_DISTANCE >= cur_vec_dist: msg = "No place for maneuvers; config distance before maneuver is " + \ str(config.MANEUVER_START_DISTANCE) + " current moving vector distance is " + str(cur_vec_dist) print(msg) logger.write(msg + "\n") break # compute X2 point else: dist_to_x2 = cur_vec_dist - config.MANEUVER_START_DISTANCE point_x2 = nav.get_point_on_vector(current_vector[0], current_vector[1], dist_to_x2) msg = "Point X2: " + str(point_x2) + "[X X2] size: " + str( dist_to_x2) print(msg) logger.write(msg) # move to X2 from_to = [current_vector[0], point_x2] move_to_point(from_to, used_points_history, gps, vesc_engine, smoothie, logger, client, nav, raw_angles_history) # TO DO: BUG - we checking YZ to be less than maneuver dist but robot will be in Y1, not in Y, so it's not # correct. Maybe config.MANEUVER_START_DISTANCE should be multiplied by 2 when checking that vector size is # big enough # go from X2 to Y1 point (see description in comments above) # YZ vector current_vector = [field_gps_coords[i], field_gps_coords[i + 1]] cur_vec_dist = nav.get_distance(current_vector[0], current_vector[1]) msg = "Next movement vector: " + str( current_vector) + " Vector size: " + str(cur_vec_dist) print(msg) logger.write(msg) # check if moving vector is too small for maneuvers if config.MANEUVER_START_DISTANCE >= cur_vec_dist: msg = "No place for maneuvers; config distance before maneuver is " + \ str(config.MANEUVER_START_DISTANCE) + " next moving vector distance is " + str(cur_vec_dist) print(msg) logger.write(msg + "\n") break # compute Y1 point to move (current vector is marked as XY where X2 and Y1 is corner maneuver points) # example: next side is BC which marked as XY, then point Y1 is on BC (YZ) else: dist_to_y1 = config.MANEUVER_START_DISTANCE point_y1 = nav.get_point_on_vector(current_vector[0], current_vector[1], dist_to_y1) msg = "Point Y1: " + str(point_y1) + "[Y Y1] size: " + str( dist_to_y1) print(msg) logger.write(msg) # move to Y1 from_to = [current_vector[0], point_y1] move_to_point(from_to, used_points_history, gps, vesc_engine, smoothie, logger, client, nav, raw_angles_history) msg = "Done!" print(msg) logger.write(msg + "\n") except KeyboardInterrupt: msg = "Stopped by a keyboard interrupt (Ctrl + C)" print(msg) logger.write(msg + "\n") except Exception: msg = "Exception occurred:\n" + traceback.format_exc() print(msg) logger.write(msg) finally: # save log data if len(used_points_history) > 0: save_gps_coordinates( used_points_history, "used_gps_history " + get_current_time() + ".txt") else: msg = "used_gps_history list has 0 elements!" print(msg) logger.write(msg) adapter_points_history = gps.get_last_positions_list() if len(adapter_points_history) > 0: save_gps_coordinates( adapter_points_history, "adapter_gps_history " + get_current_time() + ".txt") else: msg = "adapter_gps_history list has 0 elements!" print(msg) logger.write(msg) # close log and hardware connections logger.close() smoothie.disconnect() vesc_engine.disconnect() gps.disconnect() # close transmitting connections sensor_processor.endSession() client.closeConnection() sensor_processor.stopServer()
def main(): create_directories(config.DEBUG_IMAGES_PATH) working_zone_polygon = Polygon(config.WORKING_ZONE_POLY_POINTS) working_zone_points_cv = np.array(config.WORKING_ZONE_POLY_POINTS, np.int32).reshape((-1, 1, 2)) view_zone_polygon = Polygon(config.VIEW_ZONE_POLY_POINTS) view_zone_points_cv = np.array(config.VIEW_ZONE_POLY_POINTS, np.int32).reshape((-1, 1, 2)) nav = navigation.GPSComputing() used_points_history = [] logger_full = utility.Logger("full log " + get_current_time() + ".txt") logger_table = utility.Logger("table log " + get_current_time() + ".csv") # sensors picking report_field_names = [ 'temp_fet_filtered', 'temp_motor_filtered', 'avg_motor_current', 'avg_input_current', 'rpm', 'input_voltage' ] # QGIS and sensor data transmitting path = os.path.abspath(os.getcwd()) sensor_processor = SensorProcessing.SensorProcessing(path, 0) sensor_processor.startServer() client = socketForRTK.Client.Client(4000) time.sleep(1) if not client.connectionToServer(): msg = "Connection refused for Server RTK." print(msg) logger_full.write(msg + "\n") sensor_processor.startSession() try: msg = "Initializing..." print(msg) logger_full.write(msg + "\n") # load field coordinates if config.RECEIVE_FIELD_FROM_RTK: msg = "Loading field coordinates from RTK" logger_full.write(msg + "\n") try: field_gps_coords = load_coordinates(rtk.CURRENT_FIELD_PATH) except AttributeError: msg = "Couldn't get field file name from RTK script as it is wasn't assigned there." print(msg) logger_full.write(msg + "\n") exit(1) except FileNotFoundError: msg = "Couldn't not find " + rtk.CURRENT_FIELD_PATH + " file." print(msg) logger_full.write(msg + "\n") exit(1) if len(field_gps_coords) < 5: msg = "Expected at least 4 gps points in " + rtk.CURRENT_FIELD_PATH + ", got " + \ str(len(field_gps_coords)) print(msg) logger_full.write(msg + "\n") exit(1) field_gps_coords = nav.corner_points(field_gps_coords, config.FILTER_MAX_DIST, config.FILTER_MIN_DIST) else: msg = "Loading " + config.INPUT_GPS_FIELD_FILE logger_full.write(msg + "\n") field_gps_coords = load_coordinates( config.INPUT_GPS_FIELD_FILE) # [A, B, C, D] # check field corner points count if len(field_gps_coords) != 4: msg = "Expected 4 gps corner points, got " + str( len(field_gps_coords)) + "\nField:\n" + str(field_gps_coords) print(msg) logger_full.write(msg + "\n") exit(1) field_gps_coords = reduce_field_size(field_gps_coords, config.FIELD_REDUCE_SIZE, nav) # generate path points path_points = build_path(field_gps_coords, nav, logger_full) if len(path_points) > 0: save_gps_coordinates( path_points, "generated_path " + get_current_time() + ".txt") else: msg = "List of path points is empty, saving canceled." print(msg) logger_full.write(msg + "\n") if len(path_points) < 2: msg = "Expected at least 2 points in path, got " + str(len(path_points)) + \ " instead (1st point is starting point)." print(msg) logger_full.write(msg + "\n") exit(1) # load and connect to everything print("Loading periphery detector...") periphery_detector = detection.YoloOpenCVDetection( config.PERIPHERY_CLASSES_FILE, config.PERIPHERY_CONFIG_FILE, config.PERIPHERY_WEIGHTS_FILE, config.PERIPHERY_INPUT_SIZE, config.PERIPHERY_CONFIDENCE_THRESHOLD, config.PERIPHERY_NMS_THRESHOLD, config.PERIPHERY_DNN_BACKEND, config.PERIPHERY_DNN_TARGET) print("Loading precise detector...") precise_detector = detection.YoloOpenCVDetection( config.PRECISE_CLASSES_FILE, config.PRECISE_CONFIG_FILE, config.PRECISE_WEIGHTS_FILE, config.PRECISE_INPUT_SIZE, config.PRECISE_CONFIDENCE_THRESHOLD, config.PRECISE_NMS_THRESHOLD, config.PRECISE_DNN_BACKEND, config.PRECISE_DNN_TARGET) print("Loading smoothie...") smoothie = adapters.SmoothieAdapter(config.SMOOTHIE_HOST) print("Loading vesc...") vesc_engine = adapters.VescAdapter(config.VESC_RPM_SLOW, config.VESC_MOVING_TIME, config.VESC_ALIVE_FREQ, config.VESC_CHECK_FREQ, config.VESC_PORT, config.VESC_BAUDRATE) print("Loading gps...") gps = adapters.GPSUbloxAdapter(config.GPS_PORT, config.GPS_BAUDRATE, config.GPS_POSITIONS_TO_KEEP) # gps = stubs.GPSStub(config.GPS_PORT, config.GPS_BAUDRATE, config.GPS_POSITIONS_TO_KEEP) print("Loading camera...") camera = adapters.CameraAdapterIMX219_170( config.CROP_W_FROM, config.CROP_W_TO, config.CROP_H_FROM, config.CROP_H_TO, config.CV_ROTATE_CODE, config.ISP_DIGITAL_GAIN_RANGE_FROM, config.ISP_DIGITAL_GAIN_RANGE_TO, config.GAIN_RANGE_FROM, config.GAIN_RANGE_TO, config.EXPOSURE_TIME_RANGE_FROM, config.EXPOSURE_TIME_RANGE_TO, config.AE_LOCK, config.CAMERA_W, config.CAMERA_H, config.CAMERA_W, config.CAMERA_H, config.CAMERA_FRAMERATE, config.CAMERA_FLIP_METHOD) # set smoothie's A axis to 0 (nav turn wheels) response = smoothie.set_current_coordinates(A=0) if response != smoothie.RESPONSE_OK: msg = "Failed to set A=0 on smoothie (turning wheels init position), response message:\n" + response print(msg) logger_full.write(msg + "\n") # ask permission to start moving msg = "Initializing done. Press enter to start moving." input(msg) logger_full.write(msg + "\n") msg = 'GpsQ|Raw ang|Res ang|Ord ang|Sum ang|Distance |Adapter|Smoothie|' print(msg) logger_full.write(msg + "\n") msg = 'GpsQ,Raw ang,Res ang,Ord ang,Sum ang,Distance,Adapter,Smoothie,' for field_name in report_field_names: msg += field_name + "," msg = msg[:-1] logger_table.write(msg + "\n") # path points visiting loop msg = "Generated " + str(len(path_points)) + " points." logger_full.write(msg + "\n") for i in range(1, len(path_points)): from_to = [path_points[i - 1], path_points[i]] from_to_dist = nav.get_distance(from_to[0], from_to[1]) msg = "Current movement vector: " + str( from_to) + " Vector size: " + str(from_to_dist) # print(msg) logger_full.write(msg + "\n\n") move_to_point_and_extract( from_to, gps, vesc_engine, smoothie, camera, periphery_detector, precise_detector, client, logger_full, logger_table, report_field_names, used_points_history, config.UNDISTORTED_ZONE_RADIUS, working_zone_polygon, working_zone_points_cv, view_zone_polygon, view_zone_points_cv, config.DEBUG_IMAGES_PATH, nav) msg = "Work is done." print(msg) logger_full.write(msg + "\n") except KeyboardInterrupt: msg = "Stopped by a keyboard interrupt (Ctrl + C)\n" + traceback.format_exc( ) print(msg) logger_full.write(msg + "\n") except: msg = "Exception occurred:\n" + traceback.format_exc() print(msg) logger_full.write(msg + "\n") finally: # try emergency stop try: vesc_engine.stop_moving() except: pass print("Releasing camera...") camera.release() # save log data print("Saving positions histories...") if len(used_points_history) > 0: save_gps_coordinates( used_points_history, "used_gps_history " + get_current_time() + ".txt" ) # TODO: don't accumulate a lot of points - write each of them to file as soon as they come else: msg = "used_gps_history list has 0 elements!" print(msg) logger_full.write(msg + "\n") adapter_points_history = gps.get_last_positions_list( ) # TODO: reduce history positions to 1 to save RAM if len(adapter_points_history) > 0: save_gps_coordinates( adapter_points_history, "adapter_gps_history " + get_current_time() + ".txt") else: msg = "adapter_gps_history list has 0 elements!" print(msg) logger_full.write(msg + "\n") # close log and hardware connections print("Closing loggers...") logger_full.close() logger_table.close() print("Disconnecting hardware...") smoothie.disconnect() vesc_engine.disconnect() gps.disconnect() # close transmitting connections print("Closing transmitters...") sensor_processor.endSession() client.closeConnection() sensor_processor.stopServer() print("Safe disable is done.")
import adapters from config import config sma = adapters.SmoothieAdapter(config.SMOOTHIE_HOST) smc = sma.get_connector() tn = smc.get_telnet() def test_switch_to_relative(): r = sma.try_get_response() assert r == "" r = sma.switch_to_relative() assert r == sma.RESPONSE_OK r = sma.try_get_response() assert r == "" def test_halt(): r = sma.try_get_response() assert r == "" r = sma.halt() assert r == "ok Emergency Stop Requested - reset or M999 required to exit HALT state\r\n" r = sma.try_get_response() assert r == "" r = sma.switch_to_relative() assert r == sma.RESPONSE_ALARM_LOCK
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!")
def main(): mode = int( input(""" 1 = X 2 = X, Y 3 = X, Y, A 4 = X, Y, A, vesc forward 5 = X, Y, A, vesc forward, camera reading 6 = X, Y, A, vesc forward, camera reading, detection 7 = X, Y, A, vesc forward, camera reading, detection, gps """)) # check settings and mode if mode not in range(1, 8): print("Wrong mode. Exiting.") exit(1) if X < 0 or Y < 0 or A < 0 or F < 0: print( "Bad value detected in settings (at the script beginning). Exiting." ) exit(1) try: print("Loading smoothie...") with adapters.SmoothieAdapter(config.SMOOTHIE_HOST) as smoothie: # start threads if needed threads = [] if mode > 3: print("Loading threads...") threads.append(threading.Thread(target=__th_vesc, daemon=False)) if mode > 4: threads.append( threading.Thread(target=__th_camera, daemon=False)) if mode > 5: threads.append( threading.Thread(target=__th_detection, daemon=False)) if mode > 6: threads.append( threading.Thread(target=__th_gps, daemon=False)) print("Starting threads...") for thread in threads: thread.start() # start smoothie print("Starting smoothie movement...") res = smoothie.ext_align_cork_center(F=config.XY_F_MAX) smoothie.wait_for_all_actions_done() if res != smoothie.RESPONSE_OK: print("Smoothie cork center aligning was failed:\n", res) movement_positive = True while True: if movement_positive: movement_positive = False if mode == 1: res = smoothie.custom_move_for(F=F, X=X) if mode == 2: res = smoothie.custom_move_for(F=F, X=X, Y=Y) if mode > 2: res = smoothie.custom_move_for(F=F, X=X, Y=Y, A=A) else: movement_positive = True if mode == 1: res = smoothie.custom_move_for(F=F, X=-X) if mode == 2: res = smoothie.custom_move_for(F=F, X=-X, Y=-Y) if mode > 2: res = smoothie.custom_move_for(F=F, X=-X, Y=-Y, A=-A) smoothie.wait_for_all_actions_done() if res != smoothie.RESPONSE_OK: print("Smoothie movement failed:\n", res) except KeyboardInterrupt: print("Stop request received.") except: print("Unexpected exception occurred:") print(traceback.format_exc()) finally: print("Stopping everything, please wait for correct shutdown...") global KEEP_WORKING KEEP_WORKING = False print("Halting smoothie...") smoothie.halt() smoothie.reset() print("Waiting for threads...") for thread in threads: thread.join() print("Done.")
def main(): with adapters.SmoothieAdapter(config.SMOOTHIE_HOST) as smoothie: with adapters.VescAdapter(config.VESC_RPM_SLOW, config.VESC_MOVING_TIME, config.VESC_ALIVE_FREQ, config.VESC_CHECK_FREQ, config.VESC_PORT, config.VESC_BAUDRATE) as vesc: name = datetime.datetime.now().strftime("%d-%m-%Y %H:%M") with utility.Logger(name + '.txt') as logger: msg = 'Starting durability test...\n' logger.write(msg) print(msg) msg = 'RPM: {0}'.format(config.VESC_RPM_SLOW) logger.write(msg + "\n") print(msg) for i in range(COUNT_OF_ITERATION): msg = 'Iteration {0}...'.format(i) logger.write(msg + "\n") print(msg) try: A_rand = rnd.randint(config.A_MIN, config.A_MAX) # 1 msg = 'Start moving the steering wheels to {0}...'.format(A_rand) logger.write(msg + "\n") print(msg) response = smoothie.custom_move_to(config.A_F_MAX, A=A_rand) # if response != smoothie.RESPONSE_OK: logger.write(response + '\n') print(response) smoothie.wait_for_all_actions_done() moving_time_rand = rnd.uniform(MOVE_FORWARD_FROM, MOVE_FORWARD_TO) # 2 msg = 'Start moving the robot forward, VESC_MOVING_TIME {0}...'.format(moving_time_rand) logger.write(msg + '\n') print(msg) vesc.set_moving_time(moving_time_rand) vesc.start_moving() vesc.wait_for_stop() msg = 'Stop moving the robot forward, VESC_MOVING_TIME {0}...'.format(moving_time_rand) logger.write(msg + '\n') print(msg) for j in range(COUNT_OF_EXTRACTION): msg = 'Go to the extraction position Y min...' logger.write(msg + '\n') print(msg) response = smoothie.custom_move_to(config.XY_F_MAX, X=config.X_MAX / 2 / config.XY_COEFFICIENT_TO_MM, Y=config.Y_MIN) # if response != smoothie.RESPONSE_OK: logger.write(response + '\n') print(response) smoothie.wait_for_all_actions_done() point_rand = rnd.choice(config.IMAGE_CONTROL_POINTS_MAP) # 3 msg = 'Start the corkscrew movement to the control point {0}...'.format(point_rand) logger.write(msg + '\n') print(msg) response = smoothie.custom_move_for(config.XY_F_MAX, X=point_rand[2] / config.XY_COEFFICIENT_TO_MM, Y=point_rand[3] / config.XY_COEFFICIENT_TO_MM) # if response != smoothie.RESPONSE_OK: logger.write(response + '\n') print(response) smoothie.wait_for_all_actions_done() cur_coord = smoothie.get_adapter_current_coordinates() rand_sm_x = rnd.randint(SHIFT_MIN, SHIFT_MAX) # 4 rand_sm_y = rnd.randint(SHIFT_MIN, SHIFT_MAX) error_msg_x = smoothie.validate_value(cur_coord['X'] * config.XY_COEFFICIENT_TO_MM, rand_sm_x * config.XY_COEFFICIENT_TO_MM, "X", config.X_MIN, config.X_MAX, "X_MIN", "X_MAX") error_msg_y = smoothie.validate_value(cur_coord['Y'] * config.XY_COEFFICIENT_TO_MM, rand_sm_y * config.XY_COEFFICIENT_TO_MM, "Y", config.Y_MIN, config.Y_MAX, "Y_MIN", "Y_MAX") if error_msg_x: rand_sm_x = -rand_sm_x if error_msg_y: rand_sm_y = -rand_sm_y msg = 'Start pointing the corkscrew by {0} mm along the X axis, and {1} mm along the Y axis...'.format( rand_sm_x, rand_sm_y) logger.write(msg + '\n') print(msg) response = smoothie.custom_move_for(config.XY_F_MAX, X=rand_sm_x, Y=rand_sm_y) logger.write(response + '\n') print(response) smoothie.wait_for_all_actions_done() msg = 'Starting extraction movements...' # 5 logger.write(msg + '\n') print(msg) response = smoothie.custom_move_for(config.Z_F_EXTRACTION_DOWN, Z=config.EXTRACTION_Z) print(response) logger.write(response + "\n") smoothie.wait_for_all_actions_done() response = smoothie.ext_cork_up() print(response) logger.write(response + "\n") smoothie.wait_for_all_actions_done() except KeyboardInterrupt: msg = "Keyboard interrupt. Exiting." print(msg) logger.write(msg + "\n") exit(0) except: error = traceback.format_exc() logger.write(error + "\n") print(error) else: error = 'No software errors found' logger.write(error + '\n') print(error) msg = 'Done.' logger.write(msg + '\n') print(msg)
def main(): used_points_history = [] adapter_points_history = [] raw_angles_history = [] logger = utility.Logger("console " + get_current_time() + ".txt") # """ path = os.path.abspath(os.getcwd()) sP = SensorProcessing(path, 0) sP.startServer() client = Client(4000) time.sleep(1) if not client.connectionToServer(): msg = "Connection refused for Server RTK." print(msg) logger.write(msg + "\n") sP.startSession() # """ try: nav = navigation.GPSComputing() msg = "Initializing..." print(msg) logger.write(msg + "\n") with adapters.SmoothieAdapter(config.SMOOTHIE_HOST) as smoothie: with adapters.VescAdapter(config.VESC_RPM, config.VESC_MOVING_TIME, config.VESC_ALIVE_FREQ, config.VESC_CHECK_FREQ, config.VESC_PORT, config.VESC_BAUDRATE) as vesc_engine: with adapters.GPSUbloxAdapter(config.GPS_PORT, config.GPS_BAUDRATE, config.GPS_POSITIONS_TO_KEEP) as gps: # set smoothie's A axis to 0 (nav turn wheels) response = smoothie.set_current_coordinates(A=0) if response != smoothie.RESPONSE_OK: msg = "Failed to set A=0 on smoothie (turning wheels init position), response message:\n" + response print(msg) logger.write(msg + "\n") msg = "Initializing done." print(msg) logger.write(msg + "\n") # get route (field) and save it msg = "Enter 1 to create and save field.txt points file, 2 to load existing file: " command = input(msg) msg += command logger.write(msg + "\n") # pick from gps if command == "1": msg = "Press enter to save point B" input(msg) logger.write(msg) point_b = gps.get_fresh_position() msg = "Point B saved." print(msg) logger.write(msg) # ask info for moving to next point and move there rpm = 11000 msg = "RPM = " + str(rpm) + "; set moving time (seconds; will start moving immediately): " moving_time = input(msg) msg += moving_time moving_time = float(moving_time) vesc_engine.set_rpm(11000) vesc_engine.set_moving_time(moving_time) vesc_engine.start_moving() vesc_engine.wait_for_stop() vesc_engine.set_moving_time(config.VESC_MOVING_TIME) vesc_engine.set_rpm(config.VESC_RPM) msg = "Press enter to save point A" input(msg) logger.write(msg) point_a = gps.get_fresh_position() msg = "Point A saved." print(msg) logger.write(msg) field_gps_coords = [point_a, point_b] save_gps_coordinates(field_gps_coords, "field " + get_current_time() + ".txt") # load from file elif command == "2": field_gps_coords = load_coordinates(config.INPUT_GPS_FIELD_FILE) else: msg = "Wrong command, exiting." print(msg) logger.write(msg + "\n") exit(1) # start moving forward msg = "Press enter to start moving" input(msg) logger.write(msg + "\n") prev_maneuver_time = time.time() prev_point = gps.get_fresh_position() vesc_engine.start_moving() # main navigation control loop while True: cur_pos = gps.get_fresh_position() used_points_history.append(cur_pos.copy()) # """ if not client.sendData("{};{}".format(cur_pos.copy()[0], cur_pos.copy()[1])): msg = "[Client] Connection closed !" print(msg) logger.write(msg + "\n") # """ if str(cur_pos) == str(prev_point): msg = "Got the same position, added to history, calculations skipped" print(msg) logger.write(msg + "\n") continue # check if arrived # TO DO: it won't work if deviation > course destination diff, as robot will be far away on some # side and will never get too close to the path ending point distance = nav.get_distance(cur_pos, field_gps_coords[1]) # reduce speed if near the target point if USE_SPEED_LIMIT: if distance <= DECREASE_SPEED_TRESHOLD: vesc_engine.apply_rpm(int(config.VESC_RPM / 2)) else: vesc_engine.apply_rpm(config.VESC_RPM) msg = "Distance to B: " + str(distance) print(msg) logger.write(msg + "\n") if distance <= config.COURSE_DESTINATION_DIFF: vesc_engine.stop_moving() msg = "Arrived (allowed destination distance difference " + \ str(config.COURSE_DESTINATION_DIFF) + " mm)" print(msg) logger.write(msg + "\n") break # 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.write(msg + "\n") msg = "Prev: " + str(prev_point) + " Cur: " + str(cur_pos) + " A: " + str(field_gps_coords[0]) \ + " B: " + str(field_gps_coords[1]) print(msg) logger.write(msg + "\n") raw_angle = nav.get_angle(prev_point, cur_pos, cur_pos, field_gps_coords[1]) if len(raw_angles_history) >= config.WINDOW: raw_angles_history.pop(0) raw_angles_history.append(raw_angle) # angle_kp = raw_angle * config.KP sum_angles = sum(raw_angles_history) angle_kp = raw_angle * config.KP + sum_angles * config.KI target_angle_sm = angle_kp * -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 (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.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.write(msg + "\n") order_angle_sm = -(config.MANEUVERS_FREQUENCY * config.A_DEGREES_PER_SECOND * config.A_ONE_DEGREE_IN_SMOOTHIE) # convert to global 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.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.write(msg + "\n") order_angle_sm = config.A_MIN msg = "Adapter wheels pos (target): " + str(ad_wheels_pos) + " Smoothie wheels pos (current): "\ + str(sm_wheels_pos) print(msg) logger.write(msg + "\n") msg = "KI: " + str(config.KI) + " Sum angles: " + str(sum_angles) + " Sum angles history: " + \ str(raw_angles_history) print(msg) logger.write(msg + "\n") msg = "KP: " + str(config.KP) + " Raw angle: " + str(raw_angle) + " Angle * KP: " + \ str(angle_kp) + " Smoothie target angle: " + str(target_angle_sm) + \ " Smoothie absolute order angle: " + str(order_angle_sm) print(msg) logger.write(msg + "\n") prev_point = cur_pos response = smoothie.nav_turn_wheels_to(order_angle_sm, config.A_F_MAX) msg = "Smoothie response: " + response print(msg) logger.write(msg) # next tick indent print() logger.write("\n") adapter_points_history = gps.get_last_positions_list() msg = "Done!" print(msg) logger.write(msg + "\n") except KeyboardInterrupt: msg = "Stopped by a keyboard interrupt (Ctrl + C)" print(msg) logger.write(msg + "\n") except Exception: msg = "Exception occurred:\n" + traceback.format_exc() print(msg) logger.write(msg) finally: if len(used_points_history) > 0: save_gps_coordinates(used_points_history, "used_gps_history " + get_current_time() + ".txt") else: msg = "used_gps_history list has 0 elements!" print(msg) logger.write(msg) if len(adapter_points_history) > 0: save_gps_coordinates(adapter_points_history, "adapter_gps_history " + get_current_time() + ".txt") else: msg = "adapter_gps_history list has 0 elements!" print(msg) logger.write(msg) logger.close() # """ sP.endSession() client.closeConnection() sP.stopServer()