def main():
create_directories(IMAGES_OUTPUT_DIR)
working_zone_points_cv = np.array(config.WORKING_ZONE_POLY_POINTS,
np.int32).reshape((-1, 1, 2))
# loop over settings ranges
for confidence in range(CONFIDENCE_FROM, CONFIDENCE_TO, CONFIDENCE_STEP):
confidence = round(confidence / 100, 2)
for nms_threshold in range(NMS_THRESHOLD_FROM, NMS_THRESHOLD_TO,
NMS_THRESHOLD_STEP):
nms_threshold = round(nms_threshold / 100, 2)
# create detector with current settings
if USE_PERIPHERY_NN:
periphery_det = detection.YoloOpenCVDetection(
config.PERIPHERY_CLASSES_FILE,
config.PERIPHERY_CONFIG_FILE,
config.PERIPHERY_WEIGHTS_FILE, config.PERIPHERY_INPUT_SIZE,
confidence, nms_threshold, config.PERIPHERY_DNN_BACKEND,
config.PERIPHERY_DNN_TARGET)
# define cur path and create dir
current_dir = IMAGES_OUTPUT_DIR + "periphery " + str(
confidence) + " " + str(nms_threshold) + get_slash()
create_directories(current_dir)
# detect and save images
det_cnt = detect_all_in_directory(periphery_det,
working_zone_points_cv,
IMAGES_INPUT_DIR,
current_dir)
os.rename(
current_dir, current_dir[:-len(get_slash())] +
" (detected " + str(det_cnt) + ")" + get_slash())
if USE_PRECISE_NN:
precise_det = detection.YoloOpenCVDetection(
config.PRECISE_CLASSES_FILE, config.PRECISE_CONFIG_FILE,
config.PRECISE_WEIGHTS_FILE, config.PRECISE_INPUT_SIZE,
confidence, nms_threshold, config.PRECISE_DNN_BACKEND,
config.PRECISE_DNN_TARGET)
# define cur path and create dir
current_dir = IMAGES_OUTPUT_DIR + "precise " + str(
confidence) + " " + str(nms_threshold) + get_slash()
create_directories(current_dir)
# detect and save images
det_cnt = detect_all_in_directory(precise_det,
working_zone_points_cv,
IMAGES_INPUT_DIR,
current_dir)
os.rename(
current_dir, current_dir[:-len(get_slash())] +
" (detected " + str(det_cnt) + ")" + get_slash())
def test_camera_detection():
detector = detection.YoloOpenCVDetection(
config.PERIPHERY_CLASSES_FILE, config.PERIPHERY_CONFIG_FILE,
config.PERIPHERY_WEIGHTS_FILE, config.PERIPHERY_INPUT_SIZE,
config.PERIPHERY_CONFIDENCE_THRESHOLD, config.PERIPHERY_NMS_THRESHOLD,
config.PERIPHERY_DNN_BACKEND, config.PERIPHERY_DNN_TARGET)
i = 1
with adapters.CameraAdapterIMX219_170(config.CROP_W_FROM, config.CROP_W_TO, config.CROP_H_FROM,
config.CROP_H_TO, config.CV_ROTATE_CODE,
config.ISP_DIGITAL_GAIN_RANGE_FROM, config.ISP_DIGITAL_GAIN_RANGE_TO,
config.GAIN_RANGE_FROM, config.GAIN_RANGE_TO,
config.EXPOSURE_TIME_RANGE_FROM, config.EXPOSURE_TIME_RANGE_TO,
config.AE_LOCK, config.CAMERA_W, config.CAMERA_H, config.CAMERA_W,
config.CAMERA_H, config.CAMERA_FRAMERATE, config.CAMERA_FLIP_METHOD) \
as camera:
print("Test started, time values are in seconds")
while True:
start_time = time.time()
frame = camera.get_image()
frame_time = time.time()
plants = detector.detect(frame)
detector_time = time.time()
print("Frame time is", frame_time - start_time,
"Detection time is", detector_time - frame_time, "Detected",
len(plants), "plants on", i, "iteration")
i += 1
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 main():
# check for input dir existence
if not os.path.isdir(DATASET_INPUT_DIR):
print(DATASET_INPUT_DIR + " is not a directory or doesn't exist.")
exit(0)
# try to get all images to process list and check is there found any images to process
in_img_paths = []
for extension in IMAGES_EXTENSIONS:
in_img_paths += glob.glob(DATASET_INPUT_DIR + "*" + extension)
if len(in_img_paths) == 0:
print("No images with specified extensions found in " +
DATASET_INPUT_DIR + " directory.")
exit(0)
print(len(in_img_paths), "images found.")
# load detector
print("Loading periphery detector...")
if config.PERIPHERY_WRAPPER == 1:
periphery_detector = detection.YoloDarknetDetector(
config.PERIPHERY_WEIGHTS_FILE, config.PERIPHERY_CONFIG_FILE,
config.PERIPHERY_DATA_FILE, config.PERIPHERY_CONFIDENCE_THRESHOLD,
config.PERIPHERY_HIER_THRESHOLD, config.PERIPHERY_NMS_THRESHOLD)
elif config.PERIPHERY_WRAPPER == 2:
periphery_detector = detection.YoloOpenCVDetection(
config.PERIPHERY_CLASSES_FILE, config.PERIPHERY_CONFIG_FILE,
config.PERIPHERY_WEIGHTS_FILE, config.PERIPHERY_INPUT_SIZE,
config.PERIPHERY_CONFIDENCE_THRESHOLD,
config.PERIPHERY_NMS_THRESHOLD, config.PERIPHERY_DNN_BACKEND,
config.PERIPHERY_DNN_TARGET)
else:
print("Wrong config.PERIPHERY_WRAPPER = " +
str(config.PERIPHERY_WRAPPER) + " code. Exiting.")
exit(0)
# process images; in_ means input
print("Current progress print frequency is 1 per", PROGRESS_PRINT_RATE,
"image(s). Starting processing...")
iter_number = 0
for in_img_path in in_img_paths:
in_img = cv.imread(in_img_path)
plants_boxes = periphery_detector.detect(in_img)
in_txt_path = in_img_path[:in_img_path.rfind(".")] + ".txt"
with open(in_txt_path, "w") as txt_file:
plant_box: detection.DetectedPlantBox
for plant_box in plants_boxes:
txt_file.write(
plant_box.get_as_yolo(return_as_text=True) + "\n")
# display progress
iter_number += 1
if iter_number % PROGRESS_PRINT_RATE == 0:
print("Processed", iter_number, "of", len(in_img_paths),
"images...")
print("Processed", iter_number, "of", len(in_img_paths), "images.\nDone.")
def detection_process():
"""This function is running as separate process, simulates YOLO and camera work"""
detector = detection.YoloOpenCVDetection()
with adapters.CameraAdapterIMX219_170() as camera:
while True:
frame = camera.get_image()
plants = detector.detect(frame)
_ = len(plants)
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 __th_detection():
print("Loading image...")
img = cv.imread("1.jpg")
print("Loading periphery detector...")
periphery_detector = detection.YoloOpenCVDetection(
config.PERIPHERY_CLASSES_FILE, config.PERIPHERY_CONFIG_FILE,
config.PERIPHERY_WEIGHTS_FILE, config.PERIPHERY_INPUT_SIZE,
config.PERIPHERY_CONFIDENCE_THRESHOLD, config.PERIPHERY_NMS_THRESHOLD,
config.PERIPHERY_DNN_BACKEND, config.PERIPHERY_DNN_TARGET)
print("Loading precise detector...")
precise_detector = detection.YoloOpenCVDetection(
config.PRECISE_CLASSES_FILE, config.PRECISE_CONFIG_FILE,
config.PRECISE_WEIGHTS_FILE, config.PRECISE_INPUT_SIZE,
config.PRECISE_CONFIDENCE_THRESHOLD, config.PRECISE_NMS_THRESHOLD,
config.PRECISE_DNN_BACKEND, config.PRECISE_DNN_TARGET)
print("Starting detections...")
while KEEP_WORKING:
per_boxes = periphery_detector.detect(img)
pre_boxes = precise_detector.detect(img)
print("Detectors are stopped correctly.")
def main():
try:
create_directories(WITH_PLANTS_DIR, WITHOUT_PLANTS_DIR)
working_zone_polygon = Polygon(config.WORKING_ZONE_POLY_POINTS)
with adapters.SmoothieAdapter(config.SMOOTHIE_HOST) as smoothie:
periphery_det = detection.YoloOpenCVDetection(
config.PERIPHERY_CLASSES_FILE, config.PERIPHERY_CONFIG_FILE,
config.PERIPHERY_WEIGHTS_FILE, config.PERIPHERY_INPUT_SIZE,
config.PERIPHERY_CONFIDENCE_THRESHOLD,
config.PERIPHERY_NMS_THRESHOLD, config.PERIPHERY_DNN_BACKEND,
config.PERIPHERY_DNN_TARGET)
with adapters.CameraAdapterIMX219_170(
config.CROP_W_FROM, config.CROP_W_TO, config.CROP_H_FROM,
config.CROP_H_TO, config.CV_ROTATE_CODE,
config.ISP_DIGITAL_GAIN_RANGE_FROM,
config.ISP_DIGITAL_GAIN_RANGE_TO, config.GAIN_RANGE_FROM,
config.GAIN_RANGE_TO, config.EXPOSURE_TIME_RANGE_FROM,
config.EXPOSURE_TIME_RANGE_TO, config.AE_LOCK,
config.CAMERA_W, config.CAMERA_H, config.CAMERA_W,
config.CAMERA_H, config.CAMERA_FRAMERATE,
config.CAMERA_FLIP_METHOD) as camera:
with adapters.VescAdapter(config.VESC_RPM_SLOW,
config.VESC_MOVING_TIME,
config.VESC_ALIVE_FREQ,
config.VESC_CHECK_FREQ,
config.VESC_PORT,
config.VESC_BAUDRATE) as vesc_engine:
counter = 1
session_label = input(
"Set session label (added to names): ")
input("Press enter to start.")
print("Data gathering started.")
while True:
counter = gather_data(smoothie, camera, periphery_det,
counter, session_label,
working_zone_polygon)
# move_forward_smoothie(1100, -5.2, smoothie) # F1100, B-5.2 = 30 cm with max speed (B-104 F1900 for min speed 30 cm) # for smoothie moving forward control
vesc_engine.start_moving()
vesc_engine.wait_for_stop()
except KeyboardInterrupt:
print("Stopped by a keyboard interrupt")
except:
print(traceback.format_exc())
finally:
smoothie.disconnect()
print("Data gathering done.", str(counter - 1),
"images collected at this session.")
def main():
sma = adapters.SmoothieAdapter(config.SMOOTHIE_HOST)
pca = adapters.PiCameraAdapter()
det = detection.YoloOpenCVDetection()
sma.ext_align_cork_center(config.XY_F_MAX)
sma.wait_for_all_actions_done()
img = pca.get_image()
plant_boxes = det.detect(img)
plant_boxes = sort_plant_boxes_dist(plant_boxes, config.CORK_CENTER_X, config.CORK_CENTER_Y)
for box in plant_boxes:
sma.ext_align_cork_center(config.XY_F_MAX)
sma.wait_for_all_actions_done()
box_x, box_y = box.get_center_points()
# if inside the working zone
if point_is_in_rect(box_x, box_y, wl, wt, wr, wb):
while True:
box_x, box_y = box.get_center_points()
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)
res = sma.custom_move_for(config.XY_F_MAX, X=sm_x, Y=sm_y)
sma.wait_for_all_actions_done()
# check if no movement errors
if res != sma.RESPONSE_OK:
print("Smoothie error occurred:", res)
exit()
# if inside undistorted zone
if point_is_in_rect(box_x, box_y, ul, ut, ur, ub):
input("Ready to plant extraction, press enter to begin")
sma.ext_do_extraction(config.Z_F_MAX)
sma.wait_for_all_actions_done()
break
# if outside undistorted zone but in working zone
else:
temp_img = pca.get_image()
temp_plant_boxes = det.detect(temp_img)
# get closest box
box = sort_plant_boxes_dist(temp_plant_boxes, config.CORK_CENTER_X, config.CORK_CENTER_Y)[0]
# if not in working zone
else:
print(str(box), "is not in working area, switching to next")
def main():
if not os.path.exists(INPUT_IMAGES_DIR):
print(INPUT_IMAGES_DIR, "is not exist.")
exit(0)
utility.create_directories(OUTPUT_FALSE_IMAGES_DIR)
slash = utility.get_path_slash()
counter = 0
empty_files = 0
print("Loading periphery detector...")
periphery_detector = detection.YoloOpenCVDetection(
"../" + config.PERIPHERY_CLASSES_FILE,
"../" + config.PERIPHERY_CONFIG_FILE,
"../" + config.PERIPHERY_WEIGHTS_FILE, config.PERIPHERY_INPUT_SIZE,
config.PERIPHERY_CONFIDENCE_THRESHOLD, config.PERIPHERY_NMS_THRESHOLD,
config.PERIPHERY_DNN_BACKEND, config.PERIPHERY_DNN_TARGET)
print("Starting to sort. Results are displayed every", PRINT_FREQUENCY,
"images")
paths_to_images = glob.glob(INPUT_IMAGES_DIR + "*.jpg")
for full_img_path in paths_to_images:
if os.stat(full_img_path).st_size > 0:
img = cv.imread(full_img_path)
plant_boxes = periphery_detector.detect(img)
if len(plant_boxes) == 0:
file_name = full_img_path.split(slash)[-1]
os.replace(full_img_path, OUTPUT_FALSE_IMAGES_DIR + file_name)
else:
empty_files += 1
counter += 1
if counter % PRINT_FREQUENCY == 0:
print("Processed", counter, "of", len(paths_to_images), "images")
print("Done. Found and passed", empty_files, "empty image files.")
def test_detection():
print("Loading detector...")
detector = detection.YoloOpenCVDetection(
config.PERIPHERY_CLASSES_FILE, config.PERIPHERY_CONFIG_FILE,
config.PERIPHERY_WEIGHTS_FILE, config.PERIPHERY_INPUT_SIZE,
config.PERIPHERY_CONFIDENCE_THRESHOLD, config.PERIPHERY_NMS_THRESHOLD,
config.PERIPHERY_DNN_BACKEND, config.PERIPHERY_DNN_TARGET)
"""
detector = detection.YoloOpenCVDetection(config.PRECISE_CLASSES_FILE, config.PRECISE_CONFIG_FILE,
config.PRECISE_WEIGHTS_FILE, config.PRECISE_INPUT_SIZE,
config.PRECISE_CONFIDENCE_THRESHOLD, config.PRECISE_NMS_THRESHOLD
config.PRECISE_DNN_BACKEND, config.PRECISE_DNN_TARGET)
"""
img = cv.imread("1.jpg")
i = 1
while True:
prev_time = time.time()
plants = detector.detect(img)
cur_time = time.time()
print("Detected", len(plants), "plants on", i, "iteration in",
cur_time - prev_time, "seconds")
i += 1
def main():
# check if input folder exists
if not os.path.exists(config.INPUT_IMG_DIR):
try:
os.mkdir(config.INPUT_IMG_DIR)
except OSError:
print("Creation of the directory %s failed" % config.INPUT_IMG_DIR)
exit()
else:
print("Successfully created the directory %s " %
config.INPUT_IMG_DIR)
# check if output folder exists
if not os.path.exists(config.OUTPUT_IMG_DIR):
try:
os.mkdir(config.OUTPUT_IMG_DIR)
except OSError:
print("Creation of the directory %s failed" %
config.OUTPUT_IMG_DIR)
exit()
else:
print("Successfully created the directory %s " %
config.OUTPUT_IMG_DIR)
print("Loading YOLO...")
det = detection.YoloOpenCVDetection()
print("Loading camera...")
with adapters.CameraAdapterIMX219_170() as camera:
print("Camera warming up...")
time.sleep(3)
print("Loading complete.")
counter = 1
sep = " "
# make photos
while True:
action = input(
"Type empty string (just hit enter key) to make photo, type anything to begin detection: "
)
if action != "":
break
image = camera.get_image()
cv.imwrite(
config.INPUT_IMG_DIR + str(datetime.datetime.now()) + sep +
str(counter) + ".jpg", image)
counter += 1
# detect plants on that photos
images = glob.glob(config.INPUT_IMG_DIR + "*.jpg")
counter = 1
for file_path in images:
print("Processing " + str(counter) + " of " + str(len(images)) +
" images")
counter += 1
img = cv.imread(file_path)
img_y_c, img_x_c = int(img.shape[0] / 2), int(img.shape[1] / 2)
boxes = det.detect(img)
for i in range(len(boxes)):
detection.draw_box(img, boxes[i])
img = draw_zones_circle(img, img_x_c, img_y_c,
undistorted_zone_radius,
working_zone_radius)
# put results in the input folder
# cv.imwrite(file_path[:-4] + " - result.jpg", img)
file_name = file_path.split("/")[-1]
cv.imwrite(config.OUTPUT_IMG_DIR + "Result " + file_name, img)
print("Done!")
def main():
create_directories(config.DEBUG_IMAGES_PATH)
working_zone_polygon = Polygon(config.WORKING_ZONE_POLY_POINTS)
working_zone_points_cv = np.array(config.WORKING_ZONE_POLY_POINTS,
np.int32).reshape((-1, 1, 2))
view_zone_polygon = Polygon(config.VIEW_ZONE_POLY_POINTS)
view_zone_points_cv = np.array(config.VIEW_ZONE_POLY_POINTS,
np.int32).reshape((-1, 1, 2))
nav = navigation.GPSComputing()
used_points_history = []
logger_full = utility.Logger("full log " + get_current_time() + ".txt")
logger_table = utility.Logger("table log " + get_current_time() + ".csv")
# sensors picking
report_field_names = [
'temp_fet_filtered', 'temp_motor_filtered', 'avg_motor_current',
'avg_input_current', 'rpm', 'input_voltage'
]
# QGIS and sensor data transmitting
path = os.path.abspath(os.getcwd())
sensor_processor = SensorProcessing.SensorProcessing(path, 0)
sensor_processor.startServer()
client = socketForRTK.Client.Client(4000)
time.sleep(1)
if not client.connectionToServer():
msg = "Connection refused for Server RTK."
print(msg)
logger_full.write(msg + "\n")
sensor_processor.startSession()
try:
msg = "Initializing..."
print(msg)
logger_full.write(msg + "\n")
# load field coordinates
if config.RECEIVE_FIELD_FROM_RTK:
msg = "Loading field coordinates from RTK"
logger_full.write(msg + "\n")
try:
field_gps_coords = load_coordinates(rtk.CURRENT_FIELD_PATH)
except AttributeError:
msg = "Couldn't get field file name from RTK script as it is wasn't assigned there."
print(msg)
logger_full.write(msg + "\n")
exit(1)
except FileNotFoundError:
msg = "Couldn't not find " + rtk.CURRENT_FIELD_PATH + " file."
print(msg)
logger_full.write(msg + "\n")
exit(1)
if len(field_gps_coords) < 5:
msg = "Expected at least 4 gps points in " + rtk.CURRENT_FIELD_PATH + ", got " + \
str(len(field_gps_coords))
print(msg)
logger_full.write(msg + "\n")
exit(1)
field_gps_coords = nav.corner_points(field_gps_coords,
config.FILTER_MAX_DIST,
config.FILTER_MIN_DIST)
else:
msg = "Loading " + config.INPUT_GPS_FIELD_FILE
logger_full.write(msg + "\n")
field_gps_coords = load_coordinates(
config.INPUT_GPS_FIELD_FILE) # [A, B, C, D]
# check field corner points count
if len(field_gps_coords) != 4:
msg = "Expected 4 gps corner points, got " + str(
len(field_gps_coords)) + "\nField:\n" + str(field_gps_coords)
print(msg)
logger_full.write(msg + "\n")
exit(1)
field_gps_coords = reduce_field_size(field_gps_coords,
config.FIELD_REDUCE_SIZE, nav)
# generate path points
path_points = build_path(field_gps_coords, nav, logger_full)
if len(path_points) > 0:
save_gps_coordinates(
path_points, "generated_path " + get_current_time() + ".txt")
else:
msg = "List of path points is empty, saving canceled."
print(msg)
logger_full.write(msg + "\n")
if len(path_points) < 2:
msg = "Expected at least 2 points in path, got " + str(len(path_points)) + \
" instead (1st point is starting point)."
print(msg)
logger_full.write(msg + "\n")
exit(1)
# load and connect to everything
print("Loading periphery detector...")
periphery_detector = detection.YoloOpenCVDetection(
config.PERIPHERY_CLASSES_FILE, config.PERIPHERY_CONFIG_FILE,
config.PERIPHERY_WEIGHTS_FILE, config.PERIPHERY_INPUT_SIZE,
config.PERIPHERY_CONFIDENCE_THRESHOLD,
config.PERIPHERY_NMS_THRESHOLD, config.PERIPHERY_DNN_BACKEND,
config.PERIPHERY_DNN_TARGET)
print("Loading precise detector...")
precise_detector = detection.YoloOpenCVDetection(
config.PRECISE_CLASSES_FILE, config.PRECISE_CONFIG_FILE,
config.PRECISE_WEIGHTS_FILE, config.PRECISE_INPUT_SIZE,
config.PRECISE_CONFIDENCE_THRESHOLD, config.PRECISE_NMS_THRESHOLD,
config.PRECISE_DNN_BACKEND, config.PRECISE_DNN_TARGET)
print("Loading smoothie...")
smoothie = adapters.SmoothieAdapter(config.SMOOTHIE_HOST)
print("Loading vesc...")
vesc_engine = adapters.VescAdapter(config.VESC_RPM_SLOW,
config.VESC_MOVING_TIME,
config.VESC_ALIVE_FREQ,
config.VESC_CHECK_FREQ,
config.VESC_PORT,
config.VESC_BAUDRATE)
print("Loading gps...")
gps = adapters.GPSUbloxAdapter(config.GPS_PORT, config.GPS_BAUDRATE,
config.GPS_POSITIONS_TO_KEEP)
# gps = stubs.GPSStub(config.GPS_PORT, config.GPS_BAUDRATE, config.GPS_POSITIONS_TO_KEEP)
print("Loading camera...")
camera = adapters.CameraAdapterIMX219_170(
config.CROP_W_FROM, config.CROP_W_TO, config.CROP_H_FROM,
config.CROP_H_TO, config.CV_ROTATE_CODE,
config.ISP_DIGITAL_GAIN_RANGE_FROM,
config.ISP_DIGITAL_GAIN_RANGE_TO, config.GAIN_RANGE_FROM,
config.GAIN_RANGE_TO, config.EXPOSURE_TIME_RANGE_FROM,
config.EXPOSURE_TIME_RANGE_TO, config.AE_LOCK, config.CAMERA_W,
config.CAMERA_H, config.CAMERA_W, config.CAMERA_H,
config.CAMERA_FRAMERATE, config.CAMERA_FLIP_METHOD)
# set smoothie's A axis to 0 (nav turn wheels)
response = smoothie.set_current_coordinates(A=0)
if response != smoothie.RESPONSE_OK:
msg = "Failed to set A=0 on smoothie (turning wheels init position), response message:\n" + response
print(msg)
logger_full.write(msg + "\n")
# ask permission to start moving
msg = "Initializing done. Press enter to start moving."
input(msg)
logger_full.write(msg + "\n")
msg = 'GpsQ|Raw ang|Res ang|Ord ang|Sum ang|Distance |Adapter|Smoothie|'
print(msg)
logger_full.write(msg + "\n")
msg = 'GpsQ,Raw ang,Res ang,Ord ang,Sum ang,Distance,Adapter,Smoothie,'
for field_name in report_field_names:
msg += field_name + ","
msg = msg[:-1]
logger_table.write(msg + "\n")
# path points visiting loop
msg = "Generated " + str(len(path_points)) + " points."
logger_full.write(msg + "\n")
for i in range(1, len(path_points)):
from_to = [path_points[i - 1], path_points[i]]
from_to_dist = nav.get_distance(from_to[0], from_to[1])
msg = "Current movement vector: " + str(
from_to) + " Vector size: " + str(from_to_dist)
# print(msg)
logger_full.write(msg + "\n\n")
move_to_point_and_extract(
from_to, gps, vesc_engine, smoothie, camera,
periphery_detector, precise_detector, client, logger_full,
logger_table, report_field_names, used_points_history,
config.UNDISTORTED_ZONE_RADIUS, working_zone_polygon,
working_zone_points_cv, view_zone_polygon, view_zone_points_cv,
config.DEBUG_IMAGES_PATH, nav)
msg = "Work is done."
print(msg)
logger_full.write(msg + "\n")
except KeyboardInterrupt:
msg = "Stopped by a keyboard interrupt (Ctrl + C)\n" + traceback.format_exc(
)
print(msg)
logger_full.write(msg + "\n")
except:
msg = "Exception occurred:\n" + traceback.format_exc()
print(msg)
logger_full.write(msg + "\n")
finally:
# try emergency stop
try:
vesc_engine.stop_moving()
except:
pass
print("Releasing camera...")
camera.release()
# save log data
print("Saving positions histories...")
if len(used_points_history) > 0:
save_gps_coordinates(
used_points_history,
"used_gps_history " + get_current_time() + ".txt"
) # TODO: don't accumulate a lot of points - write each of them to file as soon as they come
else:
msg = "used_gps_history list has 0 elements!"
print(msg)
logger_full.write(msg + "\n")
adapter_points_history = gps.get_last_positions_list(
) # TODO: reduce history positions to 1 to save RAM
if len(adapter_points_history) > 0:
save_gps_coordinates(
adapter_points_history,
"adapter_gps_history " + get_current_time() + ".txt")
else:
msg = "adapter_gps_history list has 0 elements!"
print(msg)
logger_full.write(msg + "\n")
# close log and hardware connections
print("Closing loggers...")
logger_full.close()
logger_table.close()
print("Disconnecting hardware...")
smoothie.disconnect()
vesc_engine.disconnect()
gps.disconnect()
# close transmitting connections
print("Closing transmitters...")
sensor_processor.endSession()
client.closeConnection()
sensor_processor.stopServer()
print("Safe disable is done.")
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():
#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!")
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 main():
create_directories(config.DEBUG_IMAGES_PATH)
working_zone_polygon = Polygon(config.WORKING_ZONE_POLY_POINTS)
working_zone_points_cv = np.array(config.WORKING_ZONE_POLY_POINTS,
np.int32).reshape((-1, 1, 2))
view_zone_polygon = Polygon(config.VIEW_ZONE_POLY_POINTS)
view_zone_points_cv = np.array(config.VIEW_ZONE_POLY_POINTS,
np.int32).reshape((-1, 1, 2))
nav = navigation.GPSComputing()
used_points_history = []
raw_angles_history = []
logger_full = utility.Logger("full log " + get_current_time() + ".txt")
logger_table = utility.Logger("table log " + get_current_time() + ".csv")
# sensors picking
report_field_names = [
'temp_fet_filtered', 'temp_motor_filtered', 'avg_motor_current',
'avg_input_current', 'rpm', 'input_voltage'
]
report_file = open('report.csv', 'w', newline='')
report_writer = csv.DictWriter(report_file, fieldnames=report_field_names)
report_writer.writeheader()
# QGIS and sensor data transmitting
path = os.path.abspath(os.getcwd())
sensor_processor = SensorProcessing.SensorProcessing(path, 0)
sensor_processor.startServer()
client = socketForRTK.Client.Client(4000)
time.sleep(1)
if not client.connectionToServer():
msg = "Connection refused for Server RTK."
print(msg)
logger_full.write(msg + "\n")
sensor_processor.startSession()
try:
msg = "Initializing..."
print(msg)
logger_full.write(msg + "\n")
print("Loading periphery detector...")
periphery_detector = detection.YoloOpenCVDetection(
config.PERIPHERY_CLASSES_FILE, config.PERIPHERY_CONFIG_FILE,
config.PERIPHERY_WEIGHTS_FILE, config.PERIPHERY_INPUT_SIZE,
config.PERIPHERY_CONFIDENCE_THRESHOLD,
config.PERIPHERY_NMS_THRESHOLD, config.PERIPHERY_DNN_BACKEND,
config.PERIPHERY_DNN_TARGET)
print("Loading smoothie...")
smoothie = adapters.SmoothieAdapter(config.SMOOTHIE_HOST)
print("Loading vesc...")
vesc_engine = adapters.VescAdapter(config.VESC_RPM_SLOW,
config.VESC_MOVING_TIME,
config.VESC_ALIVE_FREQ,
config.VESC_CHECK_FREQ,
config.VESC_PORT,
config.VESC_BAUDRATE)
print("Loading GPS...")
gps = adapters.GPSUbloxAdapter(config.GPS_PORT, config.GPS_BAUDRATE,
config.GPS_POSITIONS_TO_KEEP)
print("Loading camera...")
camera = adapters.CameraAdapterIMX219_170(
config.CROP_W_FROM, config.CROP_W_TO, config.CROP_H_FROM,
config.CROP_H_TO, config.CV_ROTATE_CODE,
config.ISP_DIGITAL_GAIN_RANGE_FROM,
config.ISP_DIGITAL_GAIN_RANGE_TO, config.GAIN_RANGE_FROM,
config.GAIN_RANGE_TO, config.EXPOSURE_TIME_RANGE_FROM,
config.EXPOSURE_TIME_RANGE_TO, config.AE_LOCK, config.CAMERA_W,
config.CAMERA_H, config.CAMERA_W, config.CAMERA_H,
config.CAMERA_FRAMERATE, config.CAMERA_FLIP_METHOD)
# set smoothie's A axis to 0 (nav turn wheels)
response = smoothie.set_current_coordinates(A=0)
if response != smoothie.RESPONSE_OK:
msg = "Failed to set A=0 on smoothie (turning wheels init position), response message:\n" + response
print(msg)
logger_full.write(msg + "\n")
# load field coordinates
field_gps_coords = load_coordinates(
config.INPUT_GPS_FIELD_FILE) # [A, B, C, D]
if len(field_gps_coords) != 4:
msg = "Expected 4 gps points in " + config.INPUT_GPS_FIELD_FILE + ", got " + str(
len(field_gps_coords))
print(msg)
logger_full.write(msg + "\n")
exit(1)
# generate path points
path_points = build_path(field_gps_coords, nav, logger_full)
if len(path_points) > 0:
save_gps_coordinates(
path_points, "generated_path " + get_current_time() + ".txt")
else:
msg = "List of path points is empty, saving canceled."
print(msg)
logger_full.write(msg + "\n")
if len(path_points) < 2:
msg = "Expected at least 2 points in path, got " + str(len(path_points)) + \
" instead (1st point is starting point)."
print(msg)
logger_full.write(msg + "\n")
exit(1)
# ask permission to start moving
msg = "Initializing done. Press enter to start moving."
input(msg)
logger_full.write(msg + "\n")
msg = 'GpsQ|Raw ang|Res ang|Ord ang|Sum ang|Distance |Adapter|Smoothie|'
print(msg)
logger_full.write(msg + "\n")
msg = 'GpsQ,Raw ang,Res ang,Ord ang,Sum ang,Distance,Adapter,Smoothie,'
for field_name in report_field_names:
msg += field_name + ","
msg = msg[:-1]
logger_table.write(msg + "\n")
# path points visiting loop
for i in range(1, len(path_points)):
from_to = [path_points[i - 1], path_points[i]]
from_to_dist = nav.get_distance(from_to[0], from_to[1])
msg = "Current movement vector: " + str(
from_to) + " Vector size: " + str(from_to_dist)
# print(msg)
logger_full.write(msg + "\n")
move_to_point(from_to, used_points_history, gps, vesc_engine,
smoothie, logger_full, client, nav,
raw_angles_history, report_writer,
report_field_names, logger_table, periphery_detector,
camera, working_zone_points_cv)
msg = "Done!"
print(msg)
logger_full.write(msg + "\n")
except KeyboardInterrupt:
msg = "Stopped by a keyboard interrupt (Ctrl + C)"
print(msg)
logger_full.write(msg + "\n")
except Exception:
msg = "Exception occurred:\n" + traceback.format_exc()
print(msg)
logger_full.write(msg + "\n")
finally:
try:
print("Stopping movement...")
vesc_engine.stop_moving()
except:
pass
try:
print("Releasing camera...")
camera.release()
except:
pass
# save log data
if len(used_points_history) > 0:
save_gps_coordinates(
used_points_history,
"used_gps_history " + get_current_time() + ".txt")
else:
msg = "used_gps_history list has 0 elements!"
print(msg)
logger_full.write(msg + "\n")
adapter_points_history = gps.get_last_positions_list()
if len(adapter_points_history) > 0:
save_gps_coordinates(
adapter_points_history,
"adapter_gps_history " + get_current_time() + ".txt")
else:
msg = "adapter_gps_history list has 0 elements!"
print(msg)
logger_full.write(msg + "\n")
# close log and hardware connections
logger_full.close()
logger_table.close()
report_file.close()
smoothie.disconnect()
vesc_engine.disconnect()
gps.disconnect()
# close transmitting connections
sensor_processor.endSession()
client.closeConnection()
sensor_processor.stopServer()
