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