def process_image(self, image, video=False):
draw_image = np.copy(image)
image = self._normalize(image)
likely_windows = self._find_likely_windows(image)
if video:
likely_windows = combine_boxes(likely_windows, image.shape)
results, self.window.score = average_boxes(likely_windows, self.window.score, image.shape)
draw_image = draw_boxes(draw_image, likely_windows, color=(255, 0, 0), thick=6)
bounding_boxes = draw_boxes(draw_image, results, color=(0, 0, 255), thick=3)
if video:
return bounding_boxes
else:
return bounding_boxes, likely_windows
def main():
create_directories(IMAGES_OUTPUT_DIR)
working_zone_polygon = Polygon(WORKING_ZONE_POLY_POINTS)
working_zone_points_cv = np.array(WORKING_ZONE_POLY_POINTS, np.int32).reshape((-1, 1, 2))
counter = 1
print("Connecting to smoothie")
smoothie = create_smoothie_connection(config.SMOOTHIE_HOST)
print("Loading neural network")
detector = detection.YoloOpenCVDetection()
print("Loading camera")
with adapters.CameraAdapterIMX219_170() as camera:
time.sleep(2)
print("Moving camera to the view position")
# go to the view position (y min, x max / 2)
smoothie.custom_move_to(config.XY_F_MAX, X=config.X_MAX / 2, Y=config.Y_MIN)
smoothie.wait_for_all_actions_done()
print("Starting main loop, running in continuous movement mode")
# main control loop
while True:
scan_move_continuously(smoothie, camera, detector, working_zone_polygon,
CONTINUOUS_TRAVEL_FORCE, CONTINUOUS_TRAVEL_DISTANCE_MM *
ONE_MM_IN_SMOOTHIE)
print("Starting plants extraction in step mode")
# switch to the step mode
while True:
time.sleep(DELAY_BEFORE_TAKING_FRAME)
frame = camera.get_image()
plant_boxes = detector.detect(frame)
# save photo
if SAVE_IMAGES:
img_y_c, img_x_c = int(frame.shape[0] / 2), int(frame.shape[1] / 2)
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, plant_boxes)
save_image(IMAGES_OUTPUT_DIR, frame, counter, "View")
counter += 1
if len(plant_boxes) == 0: # start moving continuously if there's no plants left
print("No plants detected - switching to continuous mode")
break
extract_all_plants(smoothie, camera, detector, working_zone_polygon, frame, plant_boxes)
# go to the view position (y min, x max / 2)
print("Moving camera 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()
print("Moving step forward")
move_forward(STEP_TRAVEL_FORCE, STEP_DISTANCE_MM * ONE_MM_IN_SMOOTHIE, smoothie) # F1100, B-5.2 = 30 cm with max speed (B-104 F1900 for min speed 30 cm)
smoothie.wait_for_all_actions_done()
def debug_save_image(img_output_dir, label, frame, plants_boxes,
undistorted_zone_radius, poly_zone_points_cv):
# debug image saving
if config.SAVE_DEBUG_IMAGES:
img_y_c, img_x_c = int(frame.shape[0] / 2), int(frame.shape[1] / 2)
frame = draw_zone_circle(frame, img_x_c, img_y_c,
undistorted_zone_radius)
frame = draw_zone_poly(frame, poly_zone_points_cv)
frame = detection.draw_boxes(frame, plants_boxes)
save_image(img_output_dir, frame, None, label)
def draw_bounding_box(self, image):
image = self.process._normalize(image)
likely_windows = self.process._find_likely_windows(image)
likely_windows = combine_boxes(likely_windows, image.shape)
results, self.process.window.score = average_boxes(
likely_windows, self.process.window.score, image.shape)
draw_image = draw_boxes(image,
likely_windows,
color=(255, 0, 0),
thick=6)
return draw_image
def main():
create_directories(IMAGES_OUTPUT_DIR)
working_zone_polygon = Polygon(WORKING_ZONE_POLY_POINTS)
working_zone_points_cv = np.array(WORKING_ZONE_POLY_POINTS, np.int32).reshape((-1, 1, 2))
counter = 1
print("Connecting to smoothie")
smoothie = create_smoothie_connection(config.SMOOTHIE_HOST)
print("Loading neural network")
detector = detection.YoloOpenCVDetection()
print("Loading camera")
with adapters.CameraAdapterIMX219_170() as camera:
time.sleep(2)
print("Moving camera to the view position")
# go to the view position (y min, x max / 2)
smoothie.custom_move_to(config.XY_F_MAX, X=config.X_MAX / 2, Y=config.Y_MIN)
smoothie.wait_for_all_actions_done()
print("Starting main loop")
# main control loop
while True:
for _ in range(0, SQUARE_SIDE_MM, STEP_DISTANCE_MM):
time.sleep(DELAY_BEFORE_TAKING_FRAME)
frame = camera.get_image()
plant_boxes = detector.detect(frame)
# save photo
if SAVE_IMAGES:
img_y_c, img_x_c = int(frame.shape[0] / 2), int(frame.shape[1] / 2)
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, plant_boxes)
save_image(IMAGES_OUTPUT_DIR, frame, counter, "View")
counter += 1
if len(plant_boxes) == 0:
print("No plants detected")
else:
print("Detected", len(plant_boxes), "plants")
extract_all_plants(smoothie, camera, detector, working_zone_polygon, frame, plant_boxes)
print("Moving camera to the view position")
# go to the view position (y min, x max / 2)
smoothie.custom_move_to(config.XY_F_MAX, X=config.X_MAX / 2, Y=config.Y_MIN)
smoothie.wait_for_all_actions_done()
print("Moving step forward")
move_forward(STEP_TRAVEL_FORCE, STEP_DISTANCE_MM * ONE_MM_IN_SMOOTHIE, smoothie) # F1100, B-5.2 = 30 cm with max speed (B-104 F1900 for min speed 30 cm)
smoothie.wait_for_all_actions_done()
# turn for 90 degrees after edge is passed
turn_90_degrees(1000, -35, 1100, -75, smoothie)
def debug_save_image(img_output_dir, label, frame, plants_boxes, undistorted_zone_radius, poly_zone_points_cv):
# TODO: data gathering temporary hardcoded
if ALLOW_GATHERING:
save_image(DATA_GATHERING_DIR, frame, None, label)
# debug image saving
if config.SAVE_DEBUG_IMAGES:
frame = draw_zone_circle(frame, config.SCENE_CENTER_X, config.SCENE_CENTER_Y, undistorted_zone_radius)
frame = draw_zone_poly(frame, poly_zone_points_cv)
frame = detection.draw_boxes(frame, plants_boxes)
save_image(img_output_dir, frame, None, label)
def encodeFrame():
global thread_lock
while True:
# Acquire thread_lock to access the global video_frame object
with thread_lock:
global video_frame, use_detector
if video_frame is None:
continue
original_frame = video_frame[config.CROP_H_FROM:config.CROP_H_TO, config.CROP_W_FROM:config.CROP_W_TO]
if use_detector:
global detector
plants_boxes = detector.detect(original_frame)
if draw_zones:
undistorted_zone_radius = config.UNDISTORTED_ZONE_RADIUS
poly_zone_points_cv = np.array(config.WORKING_ZONE_POLY_POINTS, np.int32).reshape((-1, 1, 2))
original_frame = draw_zone_circle(original_frame, config.SCENE_CENTER_X, config.SCENE_CENTER_Y, undistorted_zone_radius)
frame = draw_zone_poly(original_frame, poly_zone_points_cv)
else:
frame = original_frame
if draw_control_point:
points_map = config.IMAGE_CONTROL_POINTS_MAP
for i in range(len(points_map)):
frame = draw_zone_circle(frame, points_map[i][0],points_map[i][1], 5)
if use_detector:
frameFinal = detection.draw_boxes(frame, plants_boxes)
else:
frameFinal = frame
if use_save_detector:
global out
out.write(frameFinal)
return_key, encoded_image = cv2.imencode(".jpg", frameFinal)
if not return_key:
continue
# Output image as a byte array
yield(b'--frame\r\n' b'Content-Type: image/jpeg\r\n\r\n' +
bytearray(encoded_image) + b'\r\n')
def draw_data_in_frame(frame,
undistorted_zone_radius=None,
poly_zone_points_cv=None,
pdz_cv_rect=None,
plants_boxes=None):
if undistorted_zone_radius is not None:
frame = ImageSaver.draw_zone_circle(frame, config.SCENE_CENTER_X,
config.SCENE_CENTER_Y,
undistorted_zone_radius,
(255, 40, 162))
frame = ImageSaver.draw_zone_circle(frame, config.SCENE_CENTER_X,
config.SCENE_CENTER_Y, 5,
(255, 40, 162), -1)
if poly_zone_points_cv is not None:
frame = ImageSaver.draw_zone_poly(frame, poly_zone_points_cv,
(255, 0, 0))
if plants_boxes is not None:
frame = detection.draw_boxes(frame, plants_boxes)
if pdz_cv_rect is not None:
frame = cv.rectangle(frame, pdz_cv_rect[0], pdz_cv_rect[1],
(16, 127, 237), 3)
return frame
center = [(top[0] + h[0] // 2, left[0] + w[0] // 2),
(top[1] + h[1] // 2, left[1] + w[1] // 2)]
euc_dist = np.sqrt((center[1][0] - center[0][0])**2 +
(center[1][1] - center[0][1])**2)
inv_dist = euc_dist / (sum(h) / 2)
prod = inv_dist / p
if prod < 1.11:
violators.add(i)
violators.add(j)
return [persons[i] for i in violators]
def get_processed_img(detector, img):
persons = detector.get_persons(img)
violators = get_violators(persons)
img = draw_boxes(img, violators)
return img
if __name__ == '__main__':
img = cv2.imread('images/street.png')
detector = default_dedector()
persons = detector.get_persons(img)
violators = get_violators(persons)
img = draw_boxes(img, violators)
cv2.imshow('boxes', img)
cv2.waitKey()
print("min={}, max={}".format(np.min(image), np.max(image)))
img_boxes, heatmap = detection.find_cars(image, y_start, y_stop, scale,
svc, X_scaler, orient,
pix_per_cell, cell_per_block,
spatial_size, hist_bins)
if thresh:
heatmap = detection.apply_threshold(heatmap, 2)
if label:
# Find final boxes from heatmap using label function
labels = measurement.label(heatmap)
draw_img = detection.draw_labeled_bboxes(np.copy(image), labels)
else:
draw_img = detection.draw_boxes(np.copy(image), img_boxes)
f, (ax1, ax2) = plt.subplots(1, 2, figsize=(14, 7))
ax1.imshow(draw_img)
ax1.set_title("Car Positions", fontsize=24)
ax2.imshow(heatmap, cmap="hot")
ax2.set_title("Heat Map", fontsize=24)
if show:
plt.show()
if save:
save_file_name = "heatmap_{}{}{}".format(
"thresh_" if thresh else "", "label_" if label else "",
os.path.basename(image_file.replace(".jpg", ".png")))
save_location = "./output_images/{}".format(save_file_name)
f.savefig(save_location, bbox_inches="tight")
def get_processed_img(detector, img):
persons = detector.get_persons(img)
violators = get_violators(persons)
img = draw_boxes(img, violators)
return img
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 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,
nav: navigation.GPSComputing, working_zone_polygon,
undistorted_zone_radius, working_zone_points_cv, img_output_dir):
"""
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:
"""
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_last_position()
vesc_engine.apply_rpm(int(config.VESC_RPM_SLOW))
vesc_engine.start_moving()
# main navigation control loop
while True:
frame = camera.get_image()
plants_boxes = periphery_det.detect(frame)
# debug image saving
if config.SAVE_DEBUG_IMAGES:
img_y_c, img_x_c = int(frame.shape[0] / 2), int(frame.shape[1] / 2)
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, plants_boxes)
save_image(img_output_dir, frame, None, "(view scan)")
# stop and extract all plants if there any in working zone
if len(plants_boxes) > 0 and any_plant_in_working_zone(
plants_boxes, working_zone_polygon):
vesc_engine.stop_moving(
) # TODO: robot is not stopping instantly (maybe add here some engines backward force and sleep time for camera?)
# rescan after we stopped
frame = camera.get_image()
plants_boxes = periphery_det.detect(frame)
# debug image saving
if config.SAVE_DEBUG_IMAGES:
img_y_c, img_x_c = int(frame.shape[0] / 2), int(
frame.shape[1] / 2)
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, plants_boxes)
save_image(img_output_dir, frame, None,
"(view scan after vesc stop)")
# do extractions if there still any plants in working zone
if len(plants_boxes) > 0 and any_plant_in_working_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)
vesc_engine.start_moving()
# navigation control
cur_pos = gps.get_last_position()
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
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
# 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_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 main():
log_counter = 1
log_dir = "log/"
if not os.path.exists(log_dir):
try:
os.mkdir(log_dir)
except OSError:
print("Creation of the directory %s failed" % log_dir)
else:
print("Successfully created the directory %s " % log_dir)
smoothie = adapters.SmoothieAdapter(config.SMOOTHIE_HOST)
detector = detection.YoloOpenCVDetection()
smoothie.ext_align_cork_center(config.XY_F_MAX)
smoothie.wait_for_all_actions_done()
with adapters.CameraAdapterIMX219_170() as camera:
time.sleep(5)
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)
plant_boxes = sort_plant_boxes_dist(plant_boxes, config.CORK_CENTER_X,
config.CORK_CENTER_Y)
# check if no plants detected
if len(plant_boxes) < 1:
print("No plants detected on view scan, exiting.")
cv.imwrite(
log_dir + str(log_counter) + " starting - see no plants.jpg",
image)
exit(0)
# log
log_img = image.copy()
log_img = detection.draw_boxes(log_img, plant_boxes)
log_img = draw_zones_circle(log_img, img_x_c, img_y_c,
undistorted_zone_radius,
working_zone_radius)
cv.imwrite(log_dir + str(log_counter) + " starting.jpg", log_img)
log_counter += 1
for box in plant_boxes:
smoothie.ext_align_cork_center(
config.XY_F_MAX) # camera in real center
smoothie.wait_for_all_actions_done()
box_x, box_y = box.get_center_points()
# if inside the working zone
if is_point_in_circle(box_x, box_y, img_x_c, img_y_c,
working_zone_radius):
while True:
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,
undistorted_zone_radius):
# calculate values to move camera over a plant
sm_x = -px_to_smoohie_value(
box_x, config.CORK_CENTER_X, config.ONE_MM_IN_PX)
sm_y = px_to_smoohie_value(box_y, config.CORK_CENTER_Y,
config.ONE_MM_IN_PX)
# move camera 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 camera over plant, smoothie error occurred:",
res)
exit(1)
# move cork to the camera position
res = smoothie.custom_move_for(config.XY_F_MAX, Y=57)
smoothie.wait_for_all_actions_done()
if res != smoothie.RESPONSE_OK:
print(
"Couldn't move cork over plant, smoothie error occurred:",
res)
exit(1)
# waiting confirmation for extraction (just to make people see how it's going on)
input(
"Ready to plant extraction, press enter to begin")
# extraction, cork down
res = smoothie.custom_move_for(config.Z_F_MAX, Z=-30)
smoothie.wait_for_all_actions_done()
if res != smoothie.RESPONSE_OK:
print(
"Couldn't move the extractor down, smoothie error occurred:",
res)
exit(1)
# 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
sm_x = -px_to_smoohie_value(box_x, img_x_c,
config.ONE_MM_IN_PX)
sm_y = px_to_smoohie_value(box_y, img_y_c,
config.ONE_MM_IN_PX)
# move for a half distance, dist is not < 10
sm_x = int(sm_x / 2) if sm_x / 2 > 10 else 10
sm_y = int(sm_y / 2) if sm_y / 2 > 10 else 10
# 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)
exit(1)
# make new photo and re-detect plants
temp_img = camera.get_image()
temp_plant_boxes = detector.detect(temp_img)
# check if no plants detected
if len(temp_plant_boxes) < 1:
print(
"No plants detected (plant was in undistorted zone before), trying to move on next item"
)
temp_img = draw_zones_circle(
temp_img, img_x_c, img_y_c,
undistorted_zone_radius, working_zone_radius)
cv.imwrite(
log_dir + str(log_counter) +
" in undistorted branch - see no plants.jpg",
temp_img)
log_counter += 1
break
# log
log_img = temp_img.copy()
log_img = detection.draw_boxes(log_img,
temp_plant_boxes)
log_img = draw_zones_circle(log_img, img_x_c, img_y_c,
undistorted_zone_radius,
working_zone_radius)
cv.imwrite(
log_dir + str(log_counter) +
" in undistorted branch - all.jpg", log_img)
# 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)
# log
log_img = temp_img.copy()
log_img = detection.draw_box(log_img, box)
log_img = draw_zones_circle(log_img, img_x_c, img_y_c,
undistorted_zone_radius,
working_zone_radius)
cv.imwrite(
log_dir + str(log_counter) +
" in undistorted branch - closest.jpg", log_img)
log_counter += 1
# if not in working zone
else:
print("skipped", str(box), "(not in working area)")
print("Script executing is done.")
def main():
time.sleep(5)
log_counter = 1
if not os.path.exists(LOG_DIR):
try:
os.mkdir(LOG_DIR)
except OSError:
print("Creation of the directory %s failed" % LOG_DIR)
logging.error("Creation of the directory %s failed" % LOG_DIR)
else:
print("Successfully created the directory %s " % LOG_DIR)
logging.info("Successfully created the directory %s " % LOG_DIR)
# working zone pre-calculations
# these points list is changed for usage in matplotlib (it has differences in the coords system)
# working_zone_points_plt = list(
# map(lambda item: [item[0], config.CROP_H_TO - config.CROP_H_FROM - item[1]], WORKING_ZONE_POLY_POINTS))
working_zone_polygon = Polygon(WORKING_ZONE_POLY_POINTS)
# these points array is used for drawing zone using OpenCV
working_zone_points_cv = np.array(WORKING_ZONE_POLY_POINTS,
np.int32).reshape((-1, 1, 2))
# remove old images from log dir
print("Removing .jpg images from log directory")
logging.debug("Removing .jpg images from log directory")
clear_log_dir()
# create smoothieboard adapter (API for access and control smoothieboard)
while True:
try:
smoothie = adapters.SmoothieAdapter(config.SMOOTHIE_HOST)
print("Successfully connected to smoothie")
logging.info("Successfully connected to smoothie")
break
except OSError as error:
logging.warning(repr(error))
print(repr(error))
detector = detection.YoloOpenCVDetection()
with adapters.CameraAdapterIMX219_170() as camera:
print("Warming up the camera")
logging.debug("Warming up the camera")
time.sleep(5)
# main loop, detection and motion
while True:
logging.debug("Starting detection and motion main loop iteration")
# go to scan position
# smoothie.ext_align_cork_center(config.XY_F_MAX)
smoothie.custom_move_to(config.XY_F_MAX,
X=config.X_MAX / 2,
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)
plant_boxes = sort_plant_boxes_dist(plant_boxes, config.X_MAX / 2,
config.Y_MIN)
# check if no plants detected
if len(plant_boxes) < 1:
print("No plants detected on view scan (img " +
str(log_counter) + "), moving forward")
logging.info("No plants detected on view scan (img " +
str(log_counter) + "), moving forward")
log_img = image.copy()
log_img = draw_zones(log_img, img_x_c, img_y_c,
UNDISTORTED_ZONE_RADIUS,
working_zone_points_cv)
if config.SAVE_DEBUG_IMAGES:
cv.imwrite(
LOG_DIR + str(log_counter) +
" overview scan (see no plants).jpg", log_img)
log_counter += 1
# move forward for 30 sm
res = smoothie.custom_move_for(1000, B=5.43)
smoothie.wait_for_all_actions_done()
if res != smoothie.RESPONSE_OK:
print(
"Couldn't move forward (for 30 sm), smoothie error occurred:",
res)
logging.critical(
"Couldn't move forward (for 30 sm), smoothie error occurred: "
+ res)
# exit(1)
continue
else:
print("Found " + str(len(plant_boxes)) +
" plants on view scan (img " + str(log_counter) + ")")
logging.info("Found " + str(len(plant_boxes)) +
" plants on view scan (img " + str(log_counter) +
")")
log_img = image.copy()
log_img = draw_zones(log_img, img_x_c, img_y_c,
UNDISTORTED_ZONE_RADIUS,
working_zone_points_cv)
log_img = detection.draw_boxes(log_img, plant_boxes)
if config.SAVE_DEBUG_IMAGES:
cv.imwrite(
LOG_DIR + str(log_counter) + " overview scan (see " +
str(len(plant_boxes)) + " plants).jpg", log_img)
log_counter += 1
# loop over all detected plants
for box in plant_boxes:
logging.debug("Starting loop over plants list iteration")
# smoothie.ext_align_cork_center(config.XY_F_MAX) # camera in real center
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()
"""
print("Processing plant on x=" + str(box_x) + " y=" + str(box_y))
logging.info("Processing plant on x=" + str(box_x) + " y=" + str(box_y))
"""
# if plant is in working zone and can be reached by cork
if is_point_in_poly(box_x, box_y, working_zone_polygon):
print("Plant is in working zone")
logging.info("Plant is in working zone")
while True:
print("Starting extraction loop")
logging.info("Starting extraction loop")
box_x, box_y = box.get_center_points()
print("Processing plant in x=" + str(box_x) + " y=" +
str(box_y))
logging.info("Processing plant in x=" + str(box_x) +
" y=" + str(box_y))
# if 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")
logging.info("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
print("Calculated smoothie moving coordinates X=" +
str(sm_x) + " Y=" + str(sm_y))
logging.debug(
"Calculated smoothie moving coordinates X=" +
str(sm_x) + " Y=" + str(sm_y))
ad_cur_coord = smoothie.get_adapter_current_coordinates(
)
print("Adapter coordinates: " + str(ad_cur_coord))
logging.info("Adapter coordinates: " +
str(ad_cur_coord))
sm_cur_coord = smoothie.get_smoothie_current_coordinates(
)
print("Smoothie coordinates: " + str(sm_cur_coord))
logging.info("Smoothie coordinates: " +
str(sm_cur_coord))
# move cork over a plant
print("Moving cork to the plant")
logging.info("Moving cork to the 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)
logging.critical(
"Couldn't move cork over plant, smoothie error occurred: "
+ str(res))
# exit(1)
# temp debug 1
log_img = camera.get_image()
if config.SAVE_DEBUG_IMAGES:
cv.imwrite(
LOG_DIR + str(log_counter) +
" extracting (cork in upper position).jpg",
log_img)
log_counter += 1
# extraction, cork down
print("Extracting plant (cork down)")
logging.info("Extracting plant (cork down)")
res = smoothie.custom_move_for(config.Z_F_MAX,
Z=-30)
smoothie.wait_for_all_actions_done()
if res != smoothie.RESPONSE_OK:
print(
"Couldn't move the extractor down, smoothie error occurred:",
res)
logging.critical(
"Couldn't move the extractor down, smoothie error occurred:"
+ str(res))
# exit(1)
# temp debug 2
log_img = camera.get_image()
if config.SAVE_DEBUG_IMAGES:
cv.imwrite(
LOG_DIR + str(log_counter) +
" extracting (cork in lower position).jpg",
log_img)
log_counter += 1
# extraction, cork up
print("Extracting plant (cork up)")
logging.info("Extracting plant (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)
logging.critical(
"Couldn't move the extractor up, smoothie error occurred:"
+ str(res))
# exit(1)
break
# if outside undistorted zone but in working zone
else:
print(
"Plant is outside undistorted zone, moving to")
logging.info(
"Plant is outside undistorted zone, moving to")
# 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]
debug_text = "Moving to px x=" + str(control_point[0]) + " y=" + str(control_point[1]) + \
" (control point #" + str(control_point[4]) + ")"
print(debug_text)
logging.info(debug_text)
# 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)
logging.critical(
"Couldn't move to plant, smoothie error occurred: "
+ str(res))
# exit(1)
# make new photo and re-detect plants
image = camera.get_image()
temp_plant_boxes = detector.detect(image)
# check if no plants detected
if len(temp_plant_boxes) < 1:
print(
"No plants detected (plant was in working zone before), trying to move on\
next item")
logging.info(
"No plants detected (plant was in working zone before), trying to move on\
next item")
log_img = image.copy()
log_img = draw_zones(log_img, img_x_c, img_y_c,
UNDISTORTED_ZONE_RADIUS,
working_zone_points_cv)
if config.SAVE_DEBUG_IMAGES:
cv.imwrite(
LOG_DIR + str(log_counter) +
" in working zone branch - see no plants.jpg",
log_img)
log_counter += 1
break
# log
log_img = image.copy()
log_img = draw_zones(log_img, img_x_c, img_y_c,
UNDISTORTED_ZONE_RADIUS,
working_zone_points_cv)
log_img = detection.draw_boxes(
log_img, temp_plant_boxes)
if config.SAVE_DEBUG_IMAGES:
cv.imwrite(
LOG_DIR + str(log_counter) +
" in working zone branch - all plants.jpg",
log_img)
log_counter += 1
# 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)
# log
log_img = image.copy()
log_img = draw_zones(log_img, img_x_c, img_y_c,
UNDISTORTED_ZONE_RADIUS,
working_zone_points_cv)
log_img = detection.draw_box(log_img, box)
if config.SAVE_DEBUG_IMAGES:
cv.imwrite(
LOG_DIR + str(log_counter) +
" in working zone branch - closest plant.jpg",
log_img)
log_counter += 1
# if not in working zone
else:
print("Skipped", str(box), "(not in working area)")
logging.info("Skipped " + str(box) +
" (not in working area)")
# move forward for 30 sm
res = smoothie.custom_move_for(1000, B=5.43)
smoothie.wait_for_all_actions_done()
if res != smoothie.RESPONSE_OK:
print(
"Couldn't move forward (for 30 sm), smoothie error occurred:",
res)
logging.critical(
"Couldn't move forward (for 30 sm), smoothie error occurred: "
+ str(res))
def draw_sliding_window(self, img):
windows = self.process._find_likely_windows(img)
return draw_boxes(img, windows)
def main():
#meters_multiplier = ask_meters_multiplier()
#distance, force = ask_speed_mode(meters_multiplier)
rpm = float(input("Set RPM: "))
moving_time = float(input("Set moving time (seconds): "))
print("Initializing detector...")
create_directories(WITH_PLANTS_OUTPUT_PATH, NO_PLANTS_OUTPUT_PATH,
DRAWN_BOXES_OUTPUT_PATH)
detector = detection.YoloOpenCVDetection()
print("Initializing smoothie...")
smoothie = adapters.SmoothieAdapter(config.SMOOTHIE_HOST)
# move camera to the low-center position
print("Moving camera to position...")
smoothie.custom_move_to(config.XY_F_MAX,
X=config.X_MAX / 2,
Y=config.Y_MIN)
smoothie.wait_for_all_actions_done()
print("Initializing camera...")
with CameraAdapterIMX219_170(ISP_DIGITAL_GAIN_RANGE_FROM,
ISP_DIGITAL_GAIN_RANGE_TO, GAIN_RANGE_FROM,
GAIN_RANGE_TO, EXPOSURE_TIME_RANGE_FROM,
EXPOSURE_TIME_RANGE_TO, AE_LOCK) as camera:
time.sleep(2)
counter = 1
try:
with adapters.VescAdapter(rpm, moving_time, VESC_ALIVE_FREQ,
VESC_CHECK_FREQ, VESC_PORT,
VESC_BAUDRATE) as vesc_engine:
# start moving forward
vesc_engine.start_moving()
# get and save images until keyboard interrupt
print("Starting to take photos...")
while vesc_engine.is_movement_allowed():
frame = camera.get_image()
plants = detector.detect(frame)
# sort and save photo
if len(plants) > 0:
save_image(WITH_PLANTS_OUTPUT_PATH, frame, counter,
"With plants")
if SAVE_WITH_BOXES:
detection.draw_boxes(frame, plants)
save_image(DRAWN_BOXES_OUTPUT_PATH, frame, counter,
"With boxes")
else:
save_image(NO_PLANTS_OUTPUT_PATH, frame, counter,
"No plants")
counter += 1
if counter % 100 == 0:
print("Saved", counter, "photos")
except KeyboardInterrupt:
vesc_engine.stop_moving()
print("Stopped by a keyboard interrupt (Ctrl + C)")
print("Saved", counter, "photos")
exit(0)
print("Saved", counter, "photos")
print("Done!")