def compute_x2_spiral(point_a: list, point_b: list, nav: navigation.GPSComputing, logger: utility.Logger): """ Computes p. x2 with distance + spiral interval distance from p. B. Distances are loaded from config file. Returns None if AB <= distance * 2 + interval (as there's no place for robot maneuvers). :param point_a: :param point_b: :param nav: :param logger: :return: """ cur_vec_dist = nav.get_distance(point_a, point_b) # check if moving vector is too small for maneuvers if config.MANEUVER_START_DISTANCE * 2 + config.SPIRAL_SIDES_INTERVAL >= cur_vec_dist: msg = "No place for maneuvers; Config maneuver distance is (that will be multiplied by 2): " + \ str(config.MANEUVER_START_DISTANCE) + " Config spiral interval: " + str(config.SPIRAL_SIDES_INTERVAL) + \ " Current moving vector distance is: " + str(cur_vec_dist) + " Given points are: " + str(point_a) + \ " " + str(point_b) # print(msg) logger.write(msg + "\n") return None return nav.get_point_on_vector( point_a, point_b, cur_vec_dist - config.MANEUVER_START_DISTANCE - config.SPIRAL_SIDES_INTERVAL)
def compute_x1_x2_int_points(point_a: list, point_b: list, nav: navigation.GPSComputing, logger: utility.Logger): """ Computes spiral interval points x1, x2 :param point_a: :param point_b: :param nav: :param logger: :return: """ cur_vec_dist = nav.get_distance(point_a, point_b) # check if moving vector is too small for maneuvers if config.SPIRAL_SIDES_INTERVAL * 2 >= cur_vec_dist: msg = "No place for maneuvers; Config spiral interval (that will be multiplied by 2): " + \ str(config.SPIRAL_SIDES_INTERVAL) + " Current moving vector distance is: " + str(cur_vec_dist) + \ " Given points are: " + str(point_a) + " " + str(point_b) # print(msg) logger.write(msg + "\n") return None point_x1_int = nav.get_point_on_vector(point_a, point_b, config.SPIRAL_SIDES_INTERVAL) point_x2_int = nav.get_point_on_vector( point_a, point_b, cur_vec_dist - config.SPIRAL_SIDES_INTERVAL) return point_x1_int, point_x2_int
def compute_x1_x2_points(point_a: list, point_b: list, nav: navigation.GPSComputing, logger: utility.Logger): """ Computes p. x1 with config distance from p. A and p. x2 with the same distance from p. B. Distance is loaded from config file. Returns None if AB <= that distance (as there's no place for robot maneuvers). :param point_a: :param point_b: :param nav: :param logger: :return: """ cur_vec_dist = nav.get_distance(point_a, point_b) # check if moving vector is too small for maneuvers if config.MANEUVER_START_DISTANCE * 2 >= cur_vec_dist: msg = "No place for maneuvers; config start maneuver distance is (that will be multiplied by 2): " + \ str(config.MANEUVER_START_DISTANCE) + " current moving vector distance is: " + str(cur_vec_dist) + \ " Given points are: " + str(point_a) + " " + str(point_b) # print(msg) logger.write(msg + "\n") return None, None point_x1 = nav.get_point_on_vector(point_a, point_b, config.MANEUVER_START_DISTANCE) point_x2 = nav.get_point_on_vector( point_a, point_b, cur_vec_dist - config.MANEUVER_START_DISTANCE) return point_x1, point_x2
def move_to_point_and_extract( coords_from_to: list, gps: adapters.GPSUbloxAdapter, vesc_engine: adapters.VescAdapter, smoothie: adapters.SmoothieAdapter, camera: adapters.CameraAdapterIMX219_170, periphery_det: detection.YoloOpenCVDetection, precise_det: detection.YoloOpenCVDetection, client, logger_full: utility.Logger, logger_table: utility.Logger, report_field_names, used_points_history: list, undistorted_zone_radius, working_zone_polygon, working_zone_points_cv, view_zone_polygon, view_zone_points_cv, img_output_dir, nav: navigation.GPSComputing): """ Moves to the given target point and extracts all weeds on the way. :param coords_from_to: :param gps: :param vesc_engine: :param smoothie: :param camera: :param periphery_det: :param precise_det: :param client: :param logger_full: :param logger_table: :param report_field_names: :param used_points_history: :param nav: :param working_zone_polygon: :param undistorted_zone_radius: :param working_zone_points_cv: :param img_output_dir: :return: """ vesc_engine.apply_rpm(config.VESC_RPM_SLOW) slow_mode_time = -float("inf") current_working_mode = working_mode_slow = 1 working_mode_switching = 2 working_mode_fast = 3 close_to_end = True # True if robot is close to one of current movement vector points, False otherwise raw_angles_history = [] stop_helping_point = nav.get_coordinate(coords_from_to[1], coords_from_to[0], 90, 1000) prev_maneuver_time = time.time() prev_pos = gps.get_last_position() # set camera to the Y min res = smoothie.custom_move_to(config.XY_F_MAX, X=config.X_MAX / 2 / config.XY_COEFFICIENT_TO_MM, Y=config.Y_MIN) if res != smoothie.RESPONSE_OK: msg = "INIT: Failed to move camera to Y min, smoothie response:\n" + res logger_full.write(msg + "\n") smoothie.wait_for_all_actions_done() # main navigation control loop while True: # EXTRACTION CONTROL start_t = time.time() frame = camera.get_image() frame_t = time.time() plants_boxes = periphery_det.detect(frame) per_det_t = time.time() debug_save_image( img_output_dir, "(periphery view scan M=" + str(current_working_mode) + ")", frame, plants_boxes, undistorted_zone_radius, working_zone_points_cv if current_working_mode == 1 else view_zone_points_cv) msg = "View frame time: " + str( frame_t - start_t) + "\t\tPer. det. time: " + str(per_det_t - frame_t) logger_full.write(msg + "\n") # slow mode if current_working_mode == working_mode_slow: if any_plant_in_zone(plants_boxes, working_zone_polygon): vesc_engine.stop_moving() time.sleep(0.2) start_work_t = time.time() frame = camera.get_image() frame_t = time.time() plants_boxes = precise_det.detect(frame) pre_det_t = time.time() debug_save_image(img_output_dir, "(precise view scan M=1)", frame, plants_boxes, undistorted_zone_radius, working_zone_points_cv) msg = "Work frame time: " + str( frame_t - start_work_t) + "\t\tPrec. det. time: " + str(pre_det_t - frame_t) logger_full.write(msg + "\n") if any_plant_in_zone(plants_boxes, working_zone_polygon): extract_all_plants(smoothie, camera, precise_det, working_zone_polygon, frame, plants_boxes, undistorted_zone_radius, working_zone_points_cv, img_output_dir) elif not any_plant_in_zone(plants_boxes, view_zone_polygon) and \ time.time() - slow_mode_time > config.SLOW_MODE_MIN_TIME: # set camera to the Y max res = smoothie.custom_move_to( config.XY_F_MAX, X=config.X_MAX / 2 / config.XY_COEFFICIENT_TO_MM, Y=config.Y_MAX / config.XY_COEFFICIENT_TO_MM) if res != smoothie.RESPONSE_OK: msg = "M=" + str( current_working_mode ) + ": " + "Failed to move to Y max, smoothie response:\n" + res logger_full.write(msg + "\n") smoothie.wait_for_all_actions_done() current_working_mode = working_mode_switching vesc_engine.start_moving() # switching to fast mode elif current_working_mode == working_mode_switching: if any_plant_in_zone(plants_boxes, view_zone_polygon): vesc_engine.stop_moving() # set camera to the Y min res = smoothie.custom_move_to(config.XY_F_MAX, X=config.X_MAX / 2 / config.XY_COEFFICIENT_TO_MM, Y=config.Y_MIN) if res != smoothie.RESPONSE_OK: msg = "M=" + str( current_working_mode ) + ": " + "Failed to move to Y min, smoothie response:\n" + res logger_full.write(msg + "\n") smoothie.wait_for_all_actions_done() current_working_mode = working_mode_slow slow_mode_time = time.time() elif smoothie.get_smoothie_current_coordinates(False)[ "Y"] + config.XY_COEFFICIENT_TO_MM * 20 > config.Y_MAX: current_working_mode = working_mode_fast if not close_to_end: vesc_engine.apply_rpm(config.VESC_RPM_FAST) # fast mode else: if any_plant_in_zone(plants_boxes, view_zone_polygon): vesc_engine.stop_moving() # set camera to the Y min res = smoothie.custom_move_to(config.XY_F_MAX, X=config.X_MAX / 2 / config.XY_COEFFICIENT_TO_MM, Y=config.Y_MIN) if res != smoothie.RESPONSE_OK: msg = "M=" + str( current_working_mode ) + ": " + "Failed to move to Y min, smoothie response:\n" + res logger_full.write(msg + "\n") smoothie.wait_for_all_actions_done() current_working_mode = working_mode_slow slow_mode_time = time.time() vesc_engine.apply_rpm(config.VESC_RPM_SLOW) elif close_to_end: vesc_engine.apply_rpm(config.VESC_RPM_SLOW) else: vesc_engine.apply_rpm(config.VESC_RPM_FAST) # NAVIGATION CONTROL nav_start_t = time.time() cur_pos = gps.get_last_position() if str(cur_pos) == str(prev_pos): # msg = "Got the same position, added to history, calculations skipped. Am I stuck?" # print(msg) # logger_full.write(msg + "\n") continue used_points_history.append(cur_pos.copy()) if not client.sendData("{};{}".format(cur_pos[0], cur_pos[1])): msg = "[Client] Connection closed !" print(msg) logger_full.write(msg + "\n") distance = nav.get_distance(cur_pos, coords_from_to[1]) msg = "Distance to B: " + str(distance) # print(msg) logger_full.write(msg + "\n") # check if arrived _, side = nav.get_deviation(coords_from_to[1], stop_helping_point, cur_pos) # if distance <= config.COURSE_DESTINATION_DIFF: # old way if side != 1: # TODO: maybe should use both side and distance checking methods at once vesc_engine.stop_moving() # msg = "Arrived (allowed destination distance difference " + str(config.COURSE_DESTINATION_DIFF) + " mm)" msg = "Arrived to " + str(coords_from_to[1]) # print(msg) logger_full.write(msg + "\n") break # pass by cur points which are very close to prev point to prevent angle errors when robot is staying # (too close points in the same position can produce false huge angles) if nav.get_distance(prev_pos, cur_pos) < config.PREV_CUR_POINT_MIN_DIST: continue # reduce speed if near the target point if config.USE_SPEED_LIMIT: distance_from_start = nav.get_distance(coords_from_to[0], cur_pos) close_to_end = distance < config.DECREASE_SPEED_TRESHOLD or distance_from_start < config.DECREASE_SPEED_TRESHOLD # 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 = "Prev: " + str(prev_pos) + " Cur: " + str(cur_pos) + " A: " + str(coords_from_to[0]) \ + " B: " + str(coords_from_to[1]) # print(msg) logger_full.write(msg + "\n") raw_angle = nav.get_angle(prev_pos, cur_pos, cur_pos, coords_from_to[1]) # sum(e) if len(raw_angles_history) >= config.WINDOW: raw_angles_history.pop(0) raw_angles_history.append(raw_angle) sum_angles = sum(raw_angles_history) if sum_angles > config.SUM_ANGLES_HISTORY_MAX: msg = "Sum angles " + str(sum_angles) + " is bigger than max allowed value " + \ str(config.SUM_ANGLES_HISTORY_MAX) + ", setting to " + str(config.SUM_ANGLES_HISTORY_MAX) # print(msg) logger_full.write(msg + "\n") sum_angles = config.SUM_ANGLES_HISTORY_MAX elif sum_angles < -config.SUM_ANGLES_HISTORY_MAX: msg = "Sum angles " + str(sum_angles) + " is less than min allowed value " + \ str(-config.SUM_ANGLES_HISTORY_MAX) + ", setting to " + str(-config.SUM_ANGLES_HISTORY_MAX) # print(msg) logger_full.write(msg + "\n") sum_angles = -config.SUM_ANGLES_HISTORY_MAX angle_kp_ki = raw_angle * config.KP + sum_angles * config.KI target_angle_sm = angle_kp_ki * -config.A_ONE_DEGREE_IN_SMOOTHIE # smoothie -Value == left, Value == right ad_wheels_pos = smoothie.get_adapter_current_coordinates()["A"] sm_wheels_pos = smoothie.get_smoothie_current_coordinates()["A"] # compute order angle (smoothie can't turn for huge values immediately also as cancel movement, # so we need to do nav. actions in steps) order_angle_sm = target_angle_sm - ad_wheels_pos # check for out of update frequency and smoothie execution speed range (for nav wheels) if order_angle_sm > config.MANEUVERS_FREQUENCY * config.A_DEGREES_PER_SECOND * \ config.A_ONE_DEGREE_IN_SMOOTHIE: msg = "Order angle changed from " + str( order_angle_sm) + " to " + str( config.MANEUVERS_FREQUENCY * config.A_DEGREES_PER_SECOND + config.A_ONE_DEGREE_IN_SMOOTHIE ) + " due to exceeding degrees per tick allowed range." # print(msg) logger_full.write(msg + "\n") order_angle_sm = config.MANEUVERS_FREQUENCY * config.A_DEGREES_PER_SECOND * \ config.A_ONE_DEGREE_IN_SMOOTHIE elif order_angle_sm < -(config.MANEUVERS_FREQUENCY * config.A_DEGREES_PER_SECOND * config.A_ONE_DEGREE_IN_SMOOTHIE): msg = "Order angle changed from " + str( order_angle_sm) + " to " + str(-( config.MANEUVERS_FREQUENCY * config.A_DEGREES_PER_SECOND * config.A_ONE_DEGREE_IN_SMOOTHIE )) + " due to exceeding degrees per tick allowed range." # print(msg) logger_full.write(msg + "\n") order_angle_sm = -(config.MANEUVERS_FREQUENCY * config.A_DEGREES_PER_SECOND * config.A_ONE_DEGREE_IN_SMOOTHIE) # convert to global smoothie coordinates order_angle_sm += ad_wheels_pos # checking for out of smoothie supported range if order_angle_sm > config.A_MAX: msg = "Global order angle changed from " + str(order_angle_sm) + " to config.A_MAX = " + \ str(config.A_MAX) + " due to exceeding smoothie allowed values range." # print(msg) logger_full.write(msg + "\n") order_angle_sm = config.A_MAX elif order_angle_sm < config.A_MIN: msg = "Global order angle changed from " + str(order_angle_sm) + " to config.A_MIN = " + \ str(config.A_MIN) + " due to exceeding smoothie allowed values range." # print(msg) logger_full.write(msg + "\n") order_angle_sm = config.A_MIN raw_angle = round(raw_angle, 2) angle_kp_ki = round(angle_kp_ki, 2) order_angle_sm = round(order_angle_sm, 2) sum_angles = round(sum_angles, 2) distance = round(distance, 2) ad_wheels_pos = round(ad_wheels_pos, 2) sm_wheels_pos = round(sm_wheels_pos, 2) gps_quality = cur_pos[2] msg = str(gps_quality).ljust(5) + str(raw_angle).ljust(8) + str( angle_kp_ki).ljust(8) + str(order_angle_sm).ljust(8) + str( sum_angles).ljust(8) + str(distance).ljust(13) + str( ad_wheels_pos).ljust(8) + str(sm_wheels_pos).ljust(9) print(msg) logger_full.write(msg + "\n") # load sensors data to csv s = "," msg = str(gps_quality) + s + str(raw_angle) + s + str(angle_kp_ki) + s + str(order_angle_sm) + s + \ str(sum_angles) + s + str(distance) + s + str(ad_wheels_pos) + s + str(sm_wheels_pos) vesc_data = vesc_engine.get_sensors_data(report_field_names) if vesc_data is not None: msg += s for key in vesc_data: msg += str(vesc_data[key]) + s msg = msg[:-1] logger_table.write(msg + "\n") prev_pos = cur_pos response = smoothie.nav_turn_wheels_to(order_angle_sm, config.A_F_MAX) if response != smoothie.RESPONSE_OK: # TODO: what if response is not ok? msg = "Smoothie response is not ok: " + response print(msg) logger_full.write(msg + "\n") msg = "Nav calc time: " + str(time.time() - nav_start_t) logger_full.write(msg + "\n\n")
def move_to_point(coords_from_to: list, used_points_history: list, gps: adapters.GPSUbloxAdapter, vesc_engine: adapters.VescAdapter, smoothie: adapters.SmoothieAdapter, logger: utility.Logger, client, nav: navigation.GPSComputing, raw_angles_history: list): """ Moves to given point. :param coords_from_to: :param used_points_history: :param gps: :param vesc_engine: :param smoothie: :param logger: :param client: :param nav: :return: :param raw_angles_history: """ raw_angles_history = [] stop_helping_point = nav.get_coordinate(coords_from_to[1], coords_from_to[0], 90, 1000) prev_maneuver_time = time.time() prev_point = gps.get_fresh_position( ) # TODO: maybe it's ok to get last position instead of waiting for fresh vesc_engine.set_rpm(int(config.VESC_RPM / 2)) 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[0], cur_pos[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 distance = nav.get_distance(cur_pos, coords_from_to[1]) msg = "Distance to B: " + str(distance) print(msg) logger.write(msg + "\n") # check if arrived _, side = nav.get_deviation(coords_from_to[1], stop_helping_point, cur_pos) # if distance <= config.COURSE_DESTINATION_DIFF: # old way if side != 1: # TODO: maybe should use both side and distance checking methods at once vesc_engine.stop_moving() # msg = "Arrived (allowed destination distance difference " + str(config.COURSE_DESTINATION_DIFF) + " mm)" msg = "Arrived." print(msg) logger.write(msg + "\n") break # reduce speed if near the target point if config.USE_SPEED_LIMIT: distance_from_start = nav.get_distance(coords_from_to[0], cur_pos) if distance < config.DECREASE_SPEED_TRESHOLD or distance_from_start < config.DECREASE_SPEED_TRESHOLD: vesc_engine.apply_rpm(int(config.VESC_RPM / 2)) else: vesc_engine.apply_rpm(config.VESC_RPM) # 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(coords_from_to[0]) \ + " B: " + str(coords_from_to[1]) print(msg) logger.write(msg + "\n") raw_angle = nav.get_angle(prev_point, cur_pos, cur_pos, coords_from_to[1]) # sum(e) if len(raw_angles_history) >= config.WINDOW: raw_angles_history.pop(0) raw_angles_history.append(raw_angle) sum_angles = sum(raw_angles_history) if sum_angles > config.SUM_ANGLES_HISTORY_MAX: msg = "Sum angles " + str(sum_angles) + " is bigger than max allowed value " + \ str(config.SUM_ANGLES_HISTORY_MAX) + ", setting to " + str(config.SUM_ANGLES_HISTORY_MAX) print(msg) logger.write(msg) sum_angles = config.SUM_ANGLES_HISTORY_MAX elif sum_angles < -config.SUM_ANGLES_HISTORY_MAX: msg = "Sum angles " + str(sum_angles) + " is less than min allowed value " + \ str(-config.SUM_ANGLES_HISTORY_MAX) + ", setting to " + str(-config.SUM_ANGLES_HISTORY_MAX) print(msg) logger.write(msg) sum_angles = -config.SUM_ANGLES_HISTORY_MAX angle_kp_ki = raw_angle * config.KP + sum_angles * config.KI target_angle_sm = angle_kp_ki * -config.A_ONE_DEGREE_IN_SMOOTHIE # smoothie -Value == left, Value == right ad_wheels_pos = smoothie.get_adapter_current_coordinates()["A"] sm_wheels_pos = smoothie.get_smoothie_current_coordinates()["A"] # compute order angle (smoothie can't turn for huge values immediately also as cancel movement, # so we need to do nav. actions in steps) order_angle_sm = target_angle_sm - ad_wheels_pos # check for out of update frequency and smoothie execution speed (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 + sum(angles) * KI: " + \ str(angle_kp_ki) + " 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")
def move_to_point( coords_from_to: list, used_points_history: list, gps: adapters.GPSUbloxAdapter, vesc_engine: adapters.VescAdapter, smoothie: adapters.SmoothieAdapter, logger_full: utility.Logger, client, nav: navigation.GPSComputing, raw_angles_history: list, report_writer, report_field_names, logger_table: utility.Logger, periphery_detector: detection.YoloOpenCVDetection, camera: adapters.CameraAdapterIMX219_170, working_zone_points_cv): """ Moves to given point. :param coords_from_to: :param used_points_history: :param gps: :param vesc_engine: :param smoothie: :param logger_full: :param client: :param nav: :param raw_angles_history: :return: """ raw_angles_history = [] stop_helping_point = nav.get_coordinate(coords_from_to[1], coords_from_to[0], 90, 1000) prev_maneuver_time = time.time() prev_point = gps.get_fresh_position( ) # TODO: maybe it's ok to get last position instead of waiting for fresh vesc_engine.set_rpm(config.VESC_RPM_SLOW) vesc_engine.start_moving() # main navigation control loop while True: # DETECTION frame = camera.get_image() plants_boxes = periphery_detector.detect(frame) if len(plants_boxes) > 0: if config.SAVE_DEBUG_IMAGES: debug_save_image(config.DEBUG_IMAGES_PATH, "", frame, plants_boxes, config.UNDISTORTED_ZONE_RADIUS, working_zone_points_cv) # NAVIGATION cur_pos = gps.get_fresh_position() used_points_history.append(cur_pos.copy()) if not client.sendData("{};{}".format(cur_pos[0], cur_pos[1])): msg = "[Client] Connection closed !" print(msg) logger_full.write(msg + "\n") if str(cur_pos) == str(prev_point): msg = "Got the same position, added to history, calculations skipped. Am I stuck?" print(msg) logger_full.write(msg + "\n") continue distance = nav.get_distance(cur_pos, coords_from_to[1]) msg = "Distance to B: " + str(distance) # print(msg) logger_full.write(msg + "\n") # check if arrived _, side = nav.get_deviation(coords_from_to[1], stop_helping_point, cur_pos) # if distance <= config.COURSE_DESTINATION_DIFF: # old way if side != 1: # TODO: maybe should use both side and distance checking methods at once vesc_engine.stop_moving() # msg = "Arrived (allowed destination distance difference " + str(config.COURSE_DESTINATION_DIFF) + " mm)" msg = "Arrived to " + str(coords_from_to[1]) # print(msg) logger_full.write(msg + "\n") break # reduce speed if near the target point if config.USE_SPEED_LIMIT: distance_from_start = nav.get_distance(coords_from_to[0], cur_pos) if distance < config.DECREASE_SPEED_TRESHOLD or distance_from_start < config.DECREASE_SPEED_TRESHOLD: vesc_engine.apply_rpm(config.VESC_RPM_SLOW) else: vesc_engine.apply_rpm(config.VESC_RPM_FAST) # do maneuvers not more often than specified value cur_time = time.time() if cur_time - prev_maneuver_time < config.MANEUVERS_FREQUENCY: continue prev_maneuver_time = cur_time msg = "Timestamp: " + str(cur_time) # print(msg) logger_full.write(msg + "\n") msg = "Prev: " + str(prev_point) + " Cur: " + str(cur_pos) + " A: " + str(coords_from_to[0]) \ + " B: " + str(coords_from_to[1]) # print(msg) logger_full.write(msg + "\n") raw_angle = nav.get_angle(prev_point, cur_pos, cur_pos, coords_from_to[1]) # sum(e) if len(raw_angles_history) >= config.WINDOW: raw_angles_history.pop(0) raw_angles_history.append(raw_angle) sum_angles = sum(raw_angles_history) if sum_angles > config.SUM_ANGLES_HISTORY_MAX: msg = "Sum angles " + str(sum_angles) + " is bigger than max allowed value " + \ str(config.SUM_ANGLES_HISTORY_MAX) + ", setting to " + str(config.SUM_ANGLES_HISTORY_MAX) # print(msg) logger_full.write(msg + "\n") sum_angles = config.SUM_ANGLES_HISTORY_MAX elif sum_angles < -config.SUM_ANGLES_HISTORY_MAX: msg = "Sum angles " + str(sum_angles) + " is less than min allowed value " + \ str(-config.SUM_ANGLES_HISTORY_MAX) + ", setting to " + str(-config.SUM_ANGLES_HISTORY_MAX) # print(msg) logger_full.write(msg + "\n") sum_angles = -config.SUM_ANGLES_HISTORY_MAX angle_kp_ki = raw_angle * config.KP + sum_angles * config.KI target_angle_sm = angle_kp_ki * -config.A_ONE_DEGREE_IN_SMOOTHIE # smoothie -Value == left, Value == right ad_wheels_pos = smoothie.get_adapter_current_coordinates()["A"] sm_wheels_pos = smoothie.get_smoothie_current_coordinates()["A"] # compute order angle (smoothie can't turn for huge values immediately also as cancel movement, # so we need to do nav. actions in steps) order_angle_sm = target_angle_sm - ad_wheels_pos # check for out of update frequency and smoothie execution speed range (for nav wheels) if order_angle_sm > config.MANEUVERS_FREQUENCY * config.A_DEGREES_PER_SECOND * \ config.A_ONE_DEGREE_IN_SMOOTHIE: msg = "Order angle changed from " + str( order_angle_sm) + " to " + str( config.MANEUVERS_FREQUENCY * config.A_DEGREES_PER_SECOND + config.A_ONE_DEGREE_IN_SMOOTHIE ) + " due to exceeding degrees per tick allowed range." # print(msg) logger_full.write(msg + "\n") order_angle_sm = config.MANEUVERS_FREQUENCY * config.A_DEGREES_PER_SECOND * \ config.A_ONE_DEGREE_IN_SMOOTHIE elif order_angle_sm < -(config.MANEUVERS_FREQUENCY * config.A_DEGREES_PER_SECOND * config.A_ONE_DEGREE_IN_SMOOTHIE): msg = "Order angle changed from " + str( order_angle_sm) + " to " + str(-( config.MANEUVERS_FREQUENCY * config.A_DEGREES_PER_SECOND * config.A_ONE_DEGREE_IN_SMOOTHIE )) + " due to exceeding degrees per tick allowed range." # print(msg) logger_full.write(msg + "\n") order_angle_sm = -(config.MANEUVERS_FREQUENCY * config.A_DEGREES_PER_SECOND * config.A_ONE_DEGREE_IN_SMOOTHIE) # convert to global smoothie coordinates order_angle_sm += ad_wheels_pos # checking for out of smoothie supported range if order_angle_sm > config.A_MAX: msg = "Global order angle changed from " + str(order_angle_sm) + " to config.A_MAX = " + \ str(config.A_MAX) + " due to exceeding smoothie allowed values range." # print(msg) logger_full.write(msg + "\n") order_angle_sm = config.A_MAX elif order_angle_sm < config.A_MIN: msg = "Global order angle changed from " + str(order_angle_sm) + " to config.A_MIN = " + \ str(config.A_MIN) + " due to exceeding smoothie allowed values range." # print(msg) logger_full.write(msg + "\n") order_angle_sm = config.A_MIN raw_angle = round(raw_angle, 2) angle_kp_ki = round(angle_kp_ki, 2) order_angle_sm = round(order_angle_sm, 2) sum_angles = round(sum_angles, 2) distance = round(distance, 2) ad_wheels_pos = round(ad_wheels_pos, 2) sm_wheels_pos = round(sm_wheels_pos, 2) gps_quality = cur_pos[2] msg = str(gps_quality).ljust(5) + str(raw_angle).ljust(8) + str( angle_kp_ki).ljust(8) + str(order_angle_sm).ljust(8) + str( sum_angles).ljust(8) + str(distance).ljust(13) + str( ad_wheels_pos).ljust(8) + str(sm_wheels_pos).ljust(9) print(msg) logger_full.write(msg + "\n") # load sensors data to csv s = "," msg = str(gps_quality) + s + str(raw_angle) + s + str(angle_kp_ki) + s + str(order_angle_sm) + s + \ str(sum_angles) + s + str(distance) + s + str(ad_wheels_pos) + s + str(sm_wheels_pos) vesc_data = vesc_engine.get_sensors_data(report_field_names) if vesc_data is not None: msg += s for key in vesc_data: msg += str(vesc_data[key]) + s msg = msg[:-1] logger_table.write(msg + "\n") prev_point = cur_pos response = smoothie.nav_turn_wheels_to(order_angle_sm, config.A_F_MAX) if response != smoothie.RESPONSE_OK: msg = "Smoothie response is not ok: " + response + "\n" print(msg) logger_full.write(msg + "\n\n")
def extract_all_plants(smoothie: adapters.SmoothieAdapter, camera: adapters.CameraAdapterIMX219_170, detector: detection.YoloOpenCVDetection, working_zone_polygon: Polygon, frame, plant_boxes: list, undistorted_zone_radius, working_zone_points_cv, img_output_dir, logger_full: utility.Logger, data_collector: datacollection.DataCollector): """Extract all plants found in current position""" msg = "Extracting " + str(len(plant_boxes)) + " plants" logger_full.write(msg + "\n") # loop over all detected plants for box in plant_boxes: # go to the extraction position Y min smoothie.custom_move_to(config.XY_F_MAX, X=config.X_MAX / 2 / config.XY_COEFFICIENT_TO_MM, Y=config.Y_MIN) smoothie.wait_for_all_actions_done() box_x, box_y = box.get_center_points() # if plant is in working zone (can be reached by cork) if is_point_in_poly(box_x, box_y, working_zone_polygon): # extraction loop for _ in range(config.EXTRACTION_TUNING_MAX_COUNT): box_x, box_y = box.get_center_points() # if plant inside undistorted zone if is_point_in_circle(box_x, box_y, config.SCENE_CENTER_X, config.SCENE_CENTER_Y, undistorted_zone_radius): msg = "Plant " + str(box) + " is in undistorted zone" logger_full.write(msg + "\n") # TODO: use plant box from precise NN for movement calculations # calculate values to move camera over a plant sm_x = px_to_smoothie_value(box_x, config.SCENE_CENTER_X, config.ONE_MM_IN_PX) sm_y = -px_to_smoothie_value(box_y, config.SCENE_CENTER_Y, config.ONE_MM_IN_PX) # swap camera and cork for extraction immediately sm_x += config.CORK_TO_CAMERA_DISTANCE_X sm_y += config.CORK_TO_CAMERA_DISTANCE_Y # move cork over a plant res = smoothie.custom_move_for(config.XY_F_MAX, X=sm_x, Y=sm_y) smoothie.wait_for_all_actions_done() if res != smoothie.RESPONSE_OK: msg = "Couldn't move cork over plant, smoothie error occurred:\n" + res logger_full.write(msg + "\n") break # debug image saving frame = camera.get_image() debug_save_image(img_output_dir, "(before first cork down)", frame, [], undistorted_zone_radius, working_zone_points_cv) # extraction if hasattr(extraction.ExtractionMethods, box.get_name()): # TODO: it's temporary log (1) msg = "Trying extractions: 5" # only Daisy implemented, it has 5 drops logger_full.write(msg + "\n") res, cork_is_stuck = getattr(extraction.ExtractionMethods, box.get_name())(smoothie, box) else: # TODO: it's temporary log (2) # 5 drops is default, also 1 center drop is possible drops = 5 if config.EXTRACTION_DEFAULT_METHOD == "five_drops_near_center" else 1 msg = "Trying extractions: " + str(drops) logger_full.write(msg + "\n") res, cork_is_stuck = getattr(extraction.ExtractionMethods, config.EXTRACTION_DEFAULT_METHOD)(smoothie, box) if res != smoothie.RESPONSE_OK: logger_full.write(res + "\n") if cork_is_stuck: # danger flag is True if smoothie couldn't pick up cork msg = "Cork is stuck! Emergency stopping." logger_full.write(msg + "\n") exit(1) else: data_collector.add_extractions_data(box.get_name(), 1) break # if outside undistorted zone but in working zone else: msg = "Plant is in working zone, trying to get closer" logger_full.write(msg + "\n") # calculate values for move camera closer to a plant control_point = get_closest_control_point(box_x, box_y, config.IMAGE_CONTROL_POINTS_MAP) # fixing cork tube view obscuring if config.AVOID_CORK_VIEW_OBSCURING: # compute target point x C_H = box_x - control_point[0] # may be negative H_x = control_point[0] + C_H target_x = H_x # compute target point y T1_y = control_point[1] - config.UNDISTORTED_ZONE_RADIUS T1_P = box_y - T1_y # always positive target_y = control_point[1] + T1_P - config.DISTANCE_FROM_UNDIST_BORDER # transfer that to millimeters sm_x = px_to_smoothie_value(target_x, control_point[0], config.ONE_MM_IN_PX) sm_y = -px_to_smoothie_value(target_y, control_point[1], config.ONE_MM_IN_PX) # move camera closer to a plant (and trying to avoid obscuring) 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: msg = "Couldn't apply cork obscuring, smoothie's response:\n" + res + "\n" + \ "(box_x: " + str(box_x) + " box_y: " + str(box_y) + " target_x: " + str(target_x) + \ " target_y: " + str(target_y) + " cp_x: " + str(control_point[0]) + " cp_y: " + \ str(control_point[1]) + ")" logger_full.write(msg + "\n") sm_x, sm_y = control_point[2], control_point[3] # move camera closer to a plant res = smoothie.custom_move_for(config.XY_F_MAX, X=sm_x, Y=sm_y) smoothie.wait_for_all_actions_done() if res != smoothie.RESPONSE_OK: msg = "Couldn't move camera closer to plant, smoothie error occurred:\n" + res logger_full.write(msg + "\n") break else: sm_x, sm_y = control_point[2], control_point[3] # move camera closer to a plant res = smoothie.custom_move_for(config.XY_F_MAX, X=sm_x, Y=sm_y) smoothie.wait_for_all_actions_done() if res != smoothie.RESPONSE_OK: msg = "Couldn't move camera closer to plant, smoothie error occurred:\n" + res logger_full.write(msg + "\n") break # make new photo and re-detect plants frame = camera.get_image() temp_plant_boxes = detector.detect(frame) # debug image saving debug_save_image(img_output_dir, "(extraction specify)", frame, temp_plant_boxes, undistorted_zone_radius, working_zone_points_cv) # check case if no plants detected if len(temp_plant_boxes) == 0: msg = "No plants detected (plant was in working zone before), trying to move on next item" logger_full.write(msg + "\n") break # get closest box (update current box from main list coordinates after moving closer) box = min_plant_box_dist(temp_plant_boxes, config.SCENE_CENTER_X, config.SCENE_CENTER_Y) else: msg = "Too much extraction attempts, trying to extract next plant if there is." logger_full.write(msg) # if not in working zone else: msg = "Skipped " + str(box) + " (not in working area)" logger_full.write(msg + "\n") # set camera back to the Y min smoothie.custom_move_to(config.XY_F_MAX, X=config.X_MAX / 2 / config.XY_COEFFICIENT_TO_MM, Y=config.Y_MIN) smoothie.wait_for_all_actions_done()