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 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 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 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 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)")