def move_forward(smoothie: adapters.SmoothieAdapter):
log_msg = "Moving forward for 30 cm"
print(log_msg)
logging.info(log_msg)
# move forward for 30 cm
res = smoothie.custom_move_for(config.B_F_MAX, B=5.43)
smoothie.wait_for_all_actions_done()
if res != smoothie.RESPONSE_OK:
log_msg = "Couldn't move forward (for 30 cm), smoothie error occurred: " + str(res)
print(log_msg)
logging.critical(log_msg)
def gather_data(smoothie: adapters.SmoothieAdapter, camera: adapters.CameraAdapterIMX219_170, counter, session_label,
working_zone_polygon):
"""Gathers photos from robot's on-board camera (no position changes for robot's body).
Saves single image if no plants were detected on view scan to separate directory.
Saves images with few positions for each detected plant if any plants detected, and saves all that images to the
separate directory."""
# go to the view position (y min, x max / 2)
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()
image = camera.get_image()
save_image(WITHOUT_PLANTS_DIR, image, counter, session_label)
counter += 1
return counter
def move_forward(force, sm_distance, smoothie: adapters.SmoothieAdapter):
"""Move forward for a specified distance with specified force"""
res = smoothie.custom_move_for(force, B=sm_distance)
if res != smoothie.RESPONSE_OK:
log_msg = "Couldn't move forward, smoothie error occurred: " + str(res)
print(log_msg)
exit(1)
def turn_90_degrees(turn_force, sm_turn_distance, moving_force, sm_moving_distance, smoothie: adapters.SmoothieAdapter):
"""Turn robot's body for 90 degrees (direction depends on sm_turn_distance: positive value for clockwise, negative
value for counterclock-wise turn)"""
# turn wheels
response = smoothie.custom_move_to(turn_force, A=sm_turn_distance)
smoothie.wait_for_all_actions_done()
if response != smoothie.RESPONSE_OK:
print("Couldn't turn wheels, smoothie error occurred:", response)
exit(1)
# go forward
move_forward(moving_force, sm_moving_distance, smoothie)
# align wheels back
response = smoothie.nav_align_wheels_center(turn_force)
if response != smoothie.RESPONSE_OK:
print("Couldn't align wheels back after turning, smoothie error occurred:", response)
exit(1)
def scan_move_continuously(smoothie: adapters.SmoothieAdapter, camera: adapters.CameraAdapterIMX219_170,
detector: detection.YoloOpenCVDetection, working_zone_polygon: Polygon, moving_force,
sm_moving_distance):
print("Moving forward continuously")
move_forward(moving_force, sm_moving_distance, smoothie)
print("Starting scanning while moving")
counter = 1
# do scans and keep moving until at least one plant detected in working zone
while True:
frame = camera.get_image()
plant_boxes = detector.detect(frame)
print("Processed", counter, "frames")
counter += 1
if len(plant_boxes) > 0:
print("Plants found, checking if any is in working zone")
for plant in plant_boxes:
plant_x, plant_y = plant.get_center_points()
if is_point_in_poly(plant_x, plant_y, working_zone_polygon):
print("Plant is in working zone found, halting smoothie")
smoothie.halt()
smoothie.reset()
print("Exiting continuous mode")
def do_extractions(smoothie: adapters.SmoothieAdapter):
for y in [100, 150]:
for x in range(10, 400, 40):
log_msg = "Moving to a plant coordinates at X=" + str(x) + " Y=" + str(y)
print(log_msg)
logging.debug(log_msg)
# move to a plant
res = smoothie.custom_move_to(config.XY_F_MAX, X=x, Y=y)
smoothie.wait_for_all_actions_done()
if res != smoothie.RESPONSE_OK:
log_msg = "Couldn't move cork over plant, smoothie error occurred: " + str(res)
print(log_msg)
logging.critical(log_msg)
# exit(1)
# extraction, cork down
log_msg = "Extracting plant (cork down)"
print(log_msg)
logging.info(log_msg)
res = smoothie.custom_move_for(config.Z_F_MAX, Z=-30)
smoothie.wait_for_all_actions_done()
if res != smoothie.RESPONSE_OK:
log_msg = "Couldn't move the extractor down, smoothie error occurred:" + str(res)
print(log_msg)
logging.critical(log_msg)
# exit(1)
# extraction, cork up
log_msg = "Extracting plant (cork up)"
print(log_msg)
logging.info(log_msg)
res = smoothie.ext_cork_up()
smoothie.wait_for_all_actions_done()
if res != smoothie.RESPONSE_OK:
log_msg = "Couldn't move the extractor up, smoothie error occurred:" + str(res)
print(log_msg)
logging.critical(log_msg)
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 extract_all_plants(smoothie: adapters.SmoothieAdapter,
camera: adapters.CameraAdapterIMX219_170,
precise_det: detection.YoloOpenCVDetection,
working_zone_polygon: Polygon, frame, plant_boxes: list,
undistorted_zone_radius, working_zone_points_cv,
img_output_dir):
"""Extract all plants found in current position"""
img_y_c, img_x_c = int(frame.shape[0] / 2), int(frame.shape[1] / 2)
# loop over all detected plants
for box in plant_boxes:
# go to the extraction position Y min
smoothie.custom_move_to(config.XY_F_MAX,
X=config.X_MAX / 2 /
config.XY_COEFFICIENT_TO_MM,
Y=config.Y_MIN)
smoothie.wait_for_all_actions_done()
box_x, box_y = box.get_center_points()
# if plant is in working zone (can be reached by cork)
if is_point_in_poly(box_x, box_y, working_zone_polygon):
# extraction loop
while True:
box_x, box_y = box.get_center_points()
# if plant inside undistorted zone
if is_point_in_circle(box_x, box_y, img_x_c, img_y_c,
config.UNDISTORTED_ZONE_RADIUS):
print("Plant is in undistorted zone")
# calculate values to move camera over a plant
sm_x = px_to_smoothie_value(box_x, img_x_c,
config.ONE_MM_IN_PX)
sm_y = -px_to_smoothie_value(box_y, img_y_c,
config.ONE_MM_IN_PX)
# swap camera and cork for extraction immediately
sm_x += config.CORK_TO_CAMERA_DISTANCE_X
sm_y += config.CORK_TO_CAMERA_DISTANCE_Y
# move cork over a plant
res = smoothie.custom_move_for(config.XY_F_MAX,
X=sm_x,
Y=sm_y)
smoothie.wait_for_all_actions_done()
if res != smoothie.RESPONSE_OK:
print(
"Couldn't move cork over plant, smoothie error occurred:",
res)
break
# extraction, cork down
res = smoothie.custom_move_for(
F=1700, Z=config.EXTRACTION_Z
) # TODO: calculation -Z depending on box size
smoothie.wait_for_all_actions_done()
if res != smoothie.RESPONSE_OK:
print(
"Couldn't move the extractor down, smoothie error occurred:",
res)
break
# extraction, cork up
res = smoothie.ext_cork_up()
smoothie.wait_for_all_actions_done()
if res != smoothie.RESPONSE_OK:
msg = "Couldn't move the extractor up, smoothie error occurred: " + res + \
"emergency exit as I don't want break corkscrew."
print(msg)
exit(1)
# Daisy additional corners extraction
if box.get_name(
) == "Daisy": # TODO: need to create flexible extraction method choosing (maybe dict of functions)
box_x_half, box_y_half = box.get_sizes()
box_x_half, box_y_half = int(box_x_half / 2 / config.ONE_MM_IN_PX), \
int(box_y_half / 2 / config.ONE_MM_IN_PX)
for x_shift, y_shift in [[-box_x_half, box_y_half],
[0, -box_y_half * 2],
[box_x_half * 2, 0],
[0, box_y_half * 2]]:
# move to the corner
response = smoothie.custom_move_for(
config.XY_F_MAX, X=x_shift, Y=y_shift)
smoothie.wait_for_all_actions_done()
if response != smoothie.RESPONSE_OK:
msg = "Aborting movement to the corner (couldn't reach): " + response
print(msg)
break
# extraction, cork down
res = smoothie.custom_move_for(
F=1700, Z=config.EXTRACTION_Z
) # TODO: calculation -Z depending on box size
smoothie.wait_for_all_actions_done()
if res != smoothie.RESPONSE_OK:
msg = "Couldn't move the extractor down, smoothie error occurred: " + res
print(msg)
break
# extraction, cork up
res = smoothie.ext_cork_up()
smoothie.wait_for_all_actions_done()
if res != smoothie.RESPONSE_OK:
msg = "Couldn't move the extractor up, smoothie error occurred: " + res + \
"emergency exit as I don't want break corkscrew."
print(msg)
exit(1)
break
# if outside undistorted zone but in working zone
else:
# calculate values for move camera closer to a plant
control_point = get_closest_control_point(
box_x, box_y, config.IMAGE_CONTROL_POINTS_MAP)
sm_x, sm_y = control_point[2], control_point[3]
# move camera closer to a plant
res = smoothie.custom_move_for(config.XY_F_MAX,
X=sm_x,
Y=sm_y)
smoothie.wait_for_all_actions_done()
if res != smoothie.RESPONSE_OK:
print(
"Couldn't move to plant, smoothie error occurred:",
res)
break
# make new photo and re-detect plants
frame = camera.get_image()
temp_plant_boxes = precise_det.detect(frame)
# check case if no plants detected
if len(temp_plant_boxes) == 0:
print(
"No plants detected (plant was in working zone before), trying to move on next item"
)
break
# debug image saving
if config.SAVE_DEBUG_IMAGES:
frame = draw_zone_circle(frame, img_x_c, img_y_c,
undistorted_zone_radius)
frame = draw_zone_poly(frame, working_zone_points_cv)
frame = detection.draw_boxes(frame, temp_plant_boxes)
save_image(img_output_dir, frame, None,
"(extraction specify)")
# get closest box (update current box from main list coordinates after moving closer)
box = min_plant_box_dist(temp_plant_boxes, img_x_c,
img_y_c)
# if not in working zone
else:
print("Skipped", str(box), "(not in working area)")
# set camera back to the Y min
smoothie.custom_move_to(config.XY_F_MAX,
X=config.X_MAX / 2 / config.XY_COEFFICIENT_TO_MM,
Y=config.Y_MIN)
smoothie.wait_for_all_actions_done()
def extract_all_plants(smoothie: adapters.SmoothieAdapter, camera: adapters.CameraAdapterIMX219_170,
detector: detection.YoloOpenCVDetection, working_zone_polygon: Polygon, image, plant_boxes: list):
"""Extract all plants found in current position"""
img_y_c, img_x_c = int(image.shape[0] / 2), int(image.shape[1] / 2)
# loop over all detected plants
for box in plant_boxes:
# go to the view position
smoothie.custom_move_to(config.XY_F_MAX, X=config.X_MAX / 2, Y=config.Y_MIN)
smoothie.wait_for_all_actions_done()
box_x, box_y = box.get_center_points()
# if plant is in working zone (can be reached by cork)
if is_point_in_poly(box_x, box_y, working_zone_polygon):
# extraction loop
while True:
box_x, box_y = box.get_center_points()
# if plant inside undistorted zone
if is_point_in_circle(box_x, box_y, img_x_c, img_y_c, UNDISTORTED_ZONE_RADIUS):
print("Plant is in undistorted zone")
# calculate values to move camera over a plant
sm_x = px_to_smoothie_value(box_x, img_x_c, config.ONE_MM_IN_PX)
sm_y = -px_to_smoothie_value(box_y, img_y_c, config.ONE_MM_IN_PX)
# swap camera and cork for extraction immediately
sm_y += CORK_CAMERA_DISTANCE
# move cork over a plant
res = smoothie.custom_move_for(config.XY_F_MAX, X=sm_x, Y=sm_y)
smoothie.wait_for_all_actions_done()
if res != smoothie.RESPONSE_OK:
print("Couldn't move cork over plant, smoothie error occurred:", res)
break
# extraction, cork down
res = smoothie.custom_move_for(F=1700, Z=-35)
smoothie.wait_for_all_actions_done()
if res != smoothie.RESPONSE_OK:
print("Couldn't move the extractor down, smoothie error occurred:", res)
break
# extraction, cork up
res = smoothie.ext_cork_up()
smoothie.wait_for_all_actions_done()
if res != smoothie.RESPONSE_OK:
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
control_point = get_closest_control_point(box_x, box_y, IMAGE_CONTROL_POINTS_MAP)
sm_x = control_point[2]
sm_y = control_point[3]
# move camera closer to a plant
res = smoothie.custom_move_for(config.XY_F_MAX, X=sm_x, Y=sm_y)
smoothie.wait_for_all_actions_done()
if res != smoothie.RESPONSE_OK:
print("Couldn't move to plant, smoothie error occurred:", res)
break
# make new photo and re-detect plants
image = camera.get_image()
temp_plant_boxes = detector.detect(image)
# check case if no plants detected
if len(temp_plant_boxes) == 0:
print("No plants detected (plant was in working zone before), trying to move on next item")
break
# get closest box (update current box from main list coordinates after moving closer)
box = min_plant_box_dist(temp_plant_boxes, img_x_c, img_y_c)
# if not in working zone
else:
print("Skipped", str(box), "(not in working area)")
def gather_data(smoothie: adapters.SmoothieAdapter,
camera: adapters.CameraAdapterIMX219_170,
detector: detection.YoloOpenCVDetection, counter,
session_label, working_zone_polygon):
"""Gathers photos from robot's on-board camera (no position changes for robot's body).
Saves single image if no plants were detected on view scan to separate directory.
Saves images with few positions for each detected plant if any plants detected, and saves all that images to the
separate directory."""
# go to the view position (y min, x max / 2)
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()
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)
# save image once if no plants detected, get more each plant positions images otherwise
if len(plant_boxes) == 0:
save_image(WITHOUT_PLANTS_DIR, image, counter, session_label)
counter += 1
# if any plants detected on the image
else:
save_image(WITH_PLANTS_DIR, image, counter, session_label)
counter += 1
# loop over all detected plants
for box in plant_boxes:
# go to the view position (y min, x max / 2)
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 and can be reached by cork
if is_point_in_poly(box_x, box_y, working_zone_polygon):
for tuning_counter in range(
config.EXTRACTION_TUNING_MAX_COUNT):
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,
config.UNDISTORTED_ZONE_RADIUS):
# get image right over plant
image = camera.get_image()
save_image(WITH_PLANTS_DIR, image, counter,
session_label)
counter += 1
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)
# try to get images over a plant and for 8 sides (left right top bot and diagonals)
# for x_shift, y_shift in [[sm_x, sm_y],[-20,0],[0,20],[20,0],[20,0],[0,-20],[0,-20],[-20,0],[-20,0]]: # do 8 additional photos around plant
for x_shift, y_shift in [[
sm_x, sm_y
]]: # only one photo right over plant
response = smoothie.custom_move_for(
config.XY_F_MAX, X=x_shift, Y=y_shift)
smoothie.wait_for_all_actions_done()
# skip this photo if couldn't change camera position
# skipping will affect that plant's other photos positions
if response != smoothie.RESPONSE_OK:
continue
image = camera.get_image()
save_image(WITH_PLANTS_DIR, image, counter,
session_label)
counter += 1
break
# if outside undistorted zone but in working zone
else:
control_point = get_closest_control_point(
box_x, box_y, config.IMAGE_CONTROL_POINTS_MAP)
sm_x = control_point[2]
sm_y = control_point[3]
# move camera closer to a plant
response = smoothie.custom_move_for(config.XY_F_MAX,
X=sm_x,
Y=sm_y)
smoothie.wait_for_all_actions_done()
if response != smoothie.RESPONSE_OK:
if tuning_counter == 0:
print(
"Something gone wrong with control point #"
+ str(control_point[4]),
"and smoothie error occurred when I tried to go closer to a plant:",
response)
break
# make a new photo and re-detect plants
temp_plant_boxes = detector.detect(camera.get_image())
# check if no plants detected
if len(temp_plant_boxes) == 0:
break
# 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)
return counter
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()
def extract_all_plants(smoothie: adapters.SmoothieAdapter,
camera: adapters.CameraAdapterIMX219_170,
detector: detection.YoloOpenCVDetection,
working_zone_polygon: Polygon, image, plant_boxes: list,
counter):
"""Extract all plants found in current position"""
img_y_c, img_x_c = int(image.shape[0] / 2), int(image.shape[1] / 2)
# loop over all detected plants
for box in plant_boxes:
# go to the view position
smoothie.custom_move_to(config.XY_F_MAX,
X=config.X_MAX / 2,
Y=config.Y_MIN)
smoothie.wait_for_all_actions_done()
box_x, box_y = box.get_center_points()
# if plant is in working zone (can be reached by cork)
if is_point_in_poly(box_x, box_y, working_zone_polygon):
# extraction loop
while True:
box_x, box_y = box.get_center_points()
# if plant inside undistorted zone
if is_point_in_circle(box_x, box_y, img_x_c, img_y_c,
UNDISTORTED_ZONE_RADIUS):
print("Plant is in undistorted zone")
# calculate values to move camera over a plant
sm_x = px_to_smoothie_value(box_x, img_x_c,
config.ONE_MM_IN_PX)
sm_y = -px_to_smoothie_value(box_y, img_y_c,
config.ONE_MM_IN_PX)
# swap camera and cork for extraction immediately
sm_y += CORK_CAMERA_DISTANCE
# move cork over a plant
res = smoothie.custom_move_for(config.XY_F_MAX,
X=sm_x,
Y=sm_y)
smoothie.wait_for_all_actions_done()
if res != smoothie.RESPONSE_OK:
print(
"Couldn't move cork over plant, smoothie error occurred:",
res)
break
# extraction, cork down
res = smoothie.custom_move_for(F=1700, Z=-42)
smoothie.wait_for_all_actions_done()
if res != smoothie.RESPONSE_OK:
print(
"Couldn't move the extractor down, smoothie error occurred:",
res)
break
# extraction, cork up
res = smoothie.ext_cork_up()
smoothie.wait_for_all_actions_done()
if res != smoothie.RESPONSE_OK:
print(
"Couldn't move the extractor up, smoothie error occurred:",
res)
exit(1)
# do photo after extraction (same counter for each robot's BODY position)
image = camera.get_image()
save_image(IMAGES_OUTPUT_DIR_HOLES, image, counter,
"After extraction")
# break
# Daisy additional corners extraction test
if box.get_name() == "Daisy":
box_x_half, box_y_half = box.get_sizes()
box_x_half, box_y_half = int(box_x_half / 2), int(
box_y_half / 2)
for x_shift, y_shift in [[-box_x_half, box_y_half],
[0, -box_y_half * 2],
[box_x_half * 2, 0],
[0, box_y_half * 2]]:
response = smoothie.custom_move_for(
config.XY_F_MAX, X=x_shift, Y=y_shift)
smoothie.wait_for_all_actions_done()
# skip this photo if couldn't change camera position
# skipping will affect that plant's other photos positions
if response != smoothie.RESPONSE_OK:
print("Aborting movement to the corner:",
response)
break
# extraction, cork down
res = smoothie.custom_move_for(F=1700, Z=-42)
smoothie.wait_for_all_actions_done()
if res != smoothie.RESPONSE_OK:
print(
"Couldn't move the extractor down, smoothie error occurred:",
res)
break
# extraction, cork up
res = smoothie.ext_cork_up()
smoothie.wait_for_all_actions_done()
if res != smoothie.RESPONSE_OK:
print(
"Couldn't move the extractor up, smoothie error occurred:",
res)
exit(1)
# do photo after extraction (same counter for each robot's BODY position)
image = camera.get_image()
save_image(IMAGES_OUTPUT_DIR_HOLES, image, counter,
"After extraction (Daisy)")
break
# if outside undistorted zone but in working zone
else:
# calculate values for move camera closer to a plant
control_point = get_closest_control_point(
box_x, box_y, IMAGE_CONTROL_POINTS_MAP)
sm_x = control_point[2]
sm_y = control_point[3]
# move camera closer to a plant
res = smoothie.custom_move_for(config.XY_F_MAX,
X=sm_x,
Y=sm_y)
smoothie.wait_for_all_actions_done()
if res != smoothie.RESPONSE_OK:
print(
"Couldn't move to plant, smoothie error occurred:",
res)
break
# make new photo and re-detect plants
image = camera.get_image()
temp_plant_boxes = detector.detect(image)
# check case if no plants detected
if len(temp_plant_boxes) == 0:
print(
"No plants detected (plant was in working zone before), trying to move on next item"
)
break
# get closest box (update current box from main list coordinates after moving closer)
box = min_plant_box_dist(temp_plant_boxes, img_x_c,
img_y_c)
# if not in working zone
else:
print("Skipped", str(box), "(not in working area)")
