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(): nav = navigation.GPSComputing() with adapters.GPSUbloxAdapter(config.GPS_PORT, config.GPS_BAUDRATE, config.GPS_POSITIONS_TO_KEEP) as gps: with adapters.VescAdapter(RPM, MOVING_TIME, config.VESC_ALIVE_FREQ, config.VESC_CHECK_FREQ, config.VESC_PORT, config.VESC_BAUDRATE) as vesc_engine: print("Getting a starting point...") starting_point = gps.get_last_position() print("Moving forward...") vesc_engine.start_moving() vesc_engine.wait_for_stop() print("Getting point A...") time.sleep(1) A = gps.get_last_position() print("Computing rest points...") B = nav.get_coordinate(A, starting_point, 180, FIELD_SIZE) C = nav.get_coordinate(B, A, 90, FIELD_SIZE) D = nav.get_coordinate(C, B, 90, FIELD_SIZE) print("Saving field.txt file...") save_gps_coordinates([A, B, C, D], "field.txt") print("Done.")
def initVesc(logger: utility.Logger): smoothie_vesc_addr = utility.get_smoothie_vesc_addresses() if "vesc" in smoothie_vesc_addr: vesc_address = smoothie_vesc_addr["vesc"] else: msg = "Couldn't get vesc's USB address!" logger.write_and_flush(msg+"\n") print(msg) exit(1) vesc_engine = adapters.VescAdapter(0, config.VESC_MOVING_TIME, config.VESC_ALIVE_FREQ, config.VESC_CHECK_FREQ, vesc_address, config.VESC_BAUDRATE) return vesc_engine
def main(): report_field_names = [ 'temp_fet_filtered', 'temp_motor_filtered', 'avg_motor_current', 'avg_input_current', 'rpm', 'input_voltage' ] print("Loading vesc...") 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: time.sleep(2) voltage = vesc.get_sensors_data(report_field_names)["input_voltage"] print(voltage)
def __th_vesc(): print("Loading vesc...") with adapters.VescAdapter(config.VESC_RPM_SLOW, float("inf"), config.VESC_ALIVE_FREQ, config.VESC_CHECK_FREQ, config.VESC_PORT, config.VESC_BAUDRATE) as vesc_engine: print("Starting vesc...") vesc_engine.start_moving() while KEEP_WORKING: time.sleep(0.3) vesc_engine.stop_moving() print("Vesc is stopped correctly.")
def main(): 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: try: rpm = int(input("Set RPM: ")) moving_time = float( input( "Set moving time (seconds; will start moving immediately): " )) vesc_engine.set_rpm(rpm) vesc_engine.set_moving_time(moving_time) vesc_engine.start_moving() vesc_engine.wait_for_stop() except KeyboardInterrupt: print("Emergency stop (KB interrupt)") vesc_engine.stop_moving() finally: vesc_engine.set_moving_time(config.VESC_MOVING_TIME) vesc_engine.set_rpm(config.VESC_RPM_SLOW) print("Done.")
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(): 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.")
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(): #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(): 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 = [] 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(): 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()